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Chapter 1 

Preview 


As an academic discipline, robotics is a relatively young field with highly am¬ 
bitious goals, the ultimate one being the creation of machines that can behave 
and think like humans. This attempt to create intelligent machines naturally 
leads us to first examine ourselves—to ask, for example, why our bodies are de¬ 
signed the way they are, how our limbs are coordinated, and how we learn and 
perform complex tasks. The sense that the fundamental questions in robotics 
are ultimately questions about ourselves is part of what makes robotics such a 
fascinating and engaging endeavor. 

Our focus in this book will be on the mechanics, planning and control of 
robot mechanisms. Robot arms are one familiar example. So are wheeled 
vehicles, as are robot arms mounted on wheeled vehicles. Basically, a mechanism 
is constructed by connecting rigid bodies, called links, together with joints, so 
that relative motion between adjacent links becomes possible. Actuation of 
the joints, typically by electric motors, then causes the robot to move and exert 
forces in desired ways. 

The links of a robot mechanism can be arranged in serial fashion, like the 
familiar open chain arm shown in Figure 1.1(a). Robot mechanisms can also 
have closed loops, such as the Stewart-Gough platform shown in Figure 1.1(b). 
In the case of an open chain, all of its joints are actuated, while in the case of 
mechanisms with closed loops only a subset of its joints may be actuated. 

Let us examine more closely the current technology behind robot mecha¬ 
nisms. The links are moved by actuators, which are typically electrically driven 
(e.g., DC or AC servo motors, stepper motors, even shape memory alloys), or 
sometimes by pneumatic or hydraulic cylinders, or internal combustion engines. 
In the case of rotating electric motors, they should ideally be lightweight, oper¬ 
ate at relatively low rotational speeds (e.g., in the range of hundreds of RPM) 
and be able to generate large forces and torques. Since most currently avail¬ 
able motors operate in the range of thousands of RPM, speed reduction devices 
with low slippage and backlash are often required. Belts, sprockets, and spur 
gears are usually not well-suited for this purpose; instead, specially designed 
low backlash gears, harmonic drives, and ball screws are used to simultaneously 
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reduce speed and amplify the delivered torque. Brakes may also be attached to 
quickly stop the robot or to maintain a stationary posture. 

Robots are also equipped with sensors to measure the position and velocity at 
the joints. For both revolute and prismatic joints, optical encoders measure the 
displacement, while tachometers measure their velocity. Forces at the links or at 
the tip can be measured using various types of force-torque sensors. Additional 
sensors may be used depending on the nature of the task, e.g., cameras, sonar 
and laser range finders to locate and measure the position and orientation of 
objects. 

This textbook is about the mechanics, motion planning, and control of such 
robots. We now provide a preview of the later chapters. 

Chapter 2: Configuration Space 




(a) An open chain industrial manipulator. (b) Stewart-Gough platform. 

Figure 1.1: Open chain and closed chain robot mechanisms. 

At its most basic level, a robot consists of rigid bodies connected by joints, 
with the joints driven by actuators. In practice the links may not be completely 
rigid, and the joints may be affected by factors such as elasticity, backlash, 
friction, and hysteresis. In this book we shall ignore these effects for the most 
part and assume all links are rigid. The most commonly found joints are revolute 
joints (allowing for rotation about the joint axis) and prismatic joints (allowing 
for linear translation along the joint axis). Revolute and prismatic joints have 
one degree of freedom (either rotation or translation); other joints, such as 
the spherical joint (also called the ball-in-socket joint), have higher degrees of 
freedom. 

In the case of an open chain robot such as the industrial manipulator of 
Figure 1.1(a), all of its joints are independently actuated. This is the essen¬ 
tial idea behind the degrees of freedom of a robot: it is the sum of all the 
independently actuated degrees of freedom of the joints. For open chains the 
degrees of freedom is obtained simply by adding up all the degrees of freedom 




3 


associated with the joints. 

For closed chains like the Stewart-Gough platform shown in Figure 1.1(b), 
the situation is somewhat more complicated. First, joints with multiple degrees 
of freedom like the spherical joint are quite common. Second, it is usually 
not possible to independently actuate all of the joints—fixing a certain set of 
joints to prescribed values automatically determines the values of the remaining 
joints. For even more complicated closed chains with multiple loops and different 
joint types, determining the degrees of freedom may not be straightforward or 
intuitive. 

A more abstract but equivalent definition of the degrees of freedom of a robot 
begins with the notion of its configuration space: a robot’s configuration 
is a complete specification of the positions and orientations of each link of a 
robot, and its configuration space is the set of all possible configurations of the 
robot. The degrees of freedom, then, is the minimum number of independent 
parameters required to specify the position and orientation of each of the links. 
Based on this definition we obtain a formula—Griibler’s formula—that relates 
the number of links and joints (including the degrees of freedom of each joint) 
comprising a robot with its degrees of freedom. 

Robot motion planning and control both begin by choosing coordinates that 
parametrize the robot’s configuration space. Often the coordinates of choice 
are the joint variables, and the configuration space can be parametrized either 
explicitly or implicitly in terms of these joint variables. Also, to grasp and 
manipulate objects, a robot is typically equipped with an end-effector, e.g., 
a mechanical hand or gripper. The task space, also called the workspace, is 
the configuration space of the end-effector. In this chapter we study the various 
ways in which the configuration and task spaces of a robot can be parametrized. 

Chapter 3: Rigid-Body Motions 

This chapter addresses the problem of how to mathematically describe the mo¬ 
tion of a rigid body moving in three-dimensional physical space. One convenient 
way is to attach a reference frame to the rigid body, and to develop a way to 
quantitatively describe the frame’s position and orientation as it moves. As a 
first step, we introduce the 3x3 matrix representation for describing a frame’s 
orientation; such a matrix is referred to as a rotation matrix. 

A rotation matrix is parametrized by three independent coordinates. The 
most natural and intuitive way to visualize a rotation matrix is in terms of its 
exponential coordinate representation. That is, given a rotation matrix R, 
there exists some unit vector Cj £ R 3 and angle 9 £ [0, ir] such that the rota¬ 
tion matrix can be obtained by rotating the identity frame (that is, the frame 
corresponding to the identity matrix) about Cj by 9. The exponential coordi¬ 
nates are defined as u = Cj9 £ R 3 , which is a three-parameter representation. 
There are several other well-known coordinate representations, e.g., Euler an¬ 
gles, Cayley-Rodrigues parameters, unit quaternions, that are further discussed 
in the appendix. 

Another reason for focusing on the exponential description of rotations is 
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that they lead directly to the exponential description of rigid-body motions. 
The latter can be viewed as a modern geometric interpretation of the classical 
screw theory, or a modern screw theory. Keeping the classical terminology (e.g., 
twists, wrenches) as much as possible, we cover in detail the linear algebraic 
constructs of screw theory, including the unified description of linear and angu¬ 
lar velocities as six-dimensional spatial velocities (twists), and an analogous 
description of three-dimensional forces and moments as six-dimensional spatial 
forces (wrenches). 

Chapter 4: Forward Kinematics 

For an open chain, the position and orientation of the end-effector are uniquely 
determined from the joint positions. This is precisely the forward kinematics 
problem for a robot: given a set of input joint values, find the output position 
and orientation of the reference frame attached to the end-effector. In this 
chapter we present the product of exponentials (PoE) formula for describing 
the forward kinematics of open chains. As the name implies, the PoE formula is 
is directly derived from the exponential coordinate representation for rigid body 
motions. Aside from providing an intuitive and easily visualizable interpretation 
of the exponential coordinates as the twists of the joint axes, the PoE formula 
offers other advantages, like eliminating the need for link frames (only the base 
frame and end-effector frame are required, and these can be chosen arbitrarily). 

In the appendix we also present the Denavit-Hartenberg (D-H) represen¬ 
tation for forward kinematics. The D-H representation uses a fewer number 
of parameters, but requires that reference frames be attached to each link fol¬ 
lowing special rules of assignment, which can be cumbersome. Details of the 
transformation from the D-H to the PoE representation are also provided in the 
appendix. 

Chapter 5: Velocity Kinematics and Statics 

Velocity kinematics refers to the relationship between joint rates and the linear 
and angular velocities of the end-effector frame. Central to velocity kinematics is 
the Jacobian of the forward kinematics. By multiplying the vector of joint rates 
by this matrix, the linear and angular velocities of the end-effector frame can be 
obtained for any given robot configuration. Kinematic singularities, which 
are configurations in which the end-effector frame loses the ability to move or 
rotate in one or more directions—imagine, for example, a two-link planar chain 
with its two links folded over each other—correspond to those configurations at 
which the Jacobian matrix fails to have maximal rank. The closely related and 
more general notion of the manipulability ellipsoid, whose shape indicates 
the ease with which the robot can move in various directions, is also derived 
from the Jacobian. 

Finally, the Jacobian is also central to static force analysis. In static equilib¬ 
rium settings, the Jacobian is used to determine what forces and torques need 
to be exerted at the input joints in order for the end-effector to apply a certain 
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force or moment in a particular direction. In this chapter we show how to obtain 
the Jacobian for general open chains, and its many practical uses in the above 
and other settings. 

Chapter 6: Inverse Kinematics 

In the inverse kinematics problem, given a desired position and orientation 
of the end-effector frame, the objective is to determine the set of joint positions 
that achieves this desired end-effector configuration. For open chain robots, the 
inverse kinematics is in general more involved than the forward kinematics: for 
a given set of joint values there usually exist a unique end-effector position and 
orientation, but for a particular end-effector position and orientation, there may 
exist multiple solutions, or even none at all. 

In this chapter we first examine a popular class of six-dof open chain struc¬ 
tures whose inverse kinematics admits a closed-form analytic solution. Iterative 
numerical algorithms are then derived for solving the inverse kinematics of gen¬ 
eral six-dof open chains. We also examine the inverse kinematics of redundant 
spatial open chains (that is, those with seven or more degrees of freedom) in 
the context of tracking a desired end-effector trajectory. For this problem, we 
present a solution for obtaining the corresponding input joint rates that relies 
on the generalized inverse of the forward kinematics Jacobian. 

Chapter 7: Kinematics of Closed Chains 

While open chains have unique forward kinematics solutions, closed chains will 
often have multiple forward kinematics solutions, and sometimes even multiple 
solutions for the inverse kinematics as well. Also, because closed chains possess 
both actuated and passive joints, the kinematic singularity analysis of closed 
chains presents subtleties not encountered in open chains. In this chapter we 
study the basic concepts and tools for the kinematic analysis of closed chains. 
We begin with a detailed case study of mechanisms like the planar five-bar 
linkage and the Stewart-Gough Platform. These results are then generalized 
into a systematic methodology for the kinematic analysis of more general closed 
chains. 


Chapter 8: Dynamics of Open Chains 

Dynamics is the study of motion taking into account the forces and torques that 
cause it. In this chapter we study the dynamics of open chain robots. Analo¬ 
gous to the notions of a robot’s forward and inverse kinematics, the forward 
dynamics problem seeks to determine the resulting joint trajectory for a given 
input joint torque profile. The inverse dynamics problem is concerned with 
determining the input joint torque profile for a desired joint trajectory. The 
dynamic equations relating the forces and torques to the motion of the robot’s 
links are given by a set of second-order ordinary differential equations. 
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We begin with a formulation of the dynamic equations for a single rigid body 
in terms of spatial velocities (twists), spatial accelerations, and spatial forces 
(wrenches). The dynamics for an open chain robot can be derived using one of 
two approaches. In the Newton-Euler approach, the equations of motion derived 
for each rigid body are merged in an appropriate fashion. Because of the open 
chain structure, the dynamics can be formulated recursively along the links. In 
the Lagrangian approach, a set of coordinates—referred to as the generalized 
coordinates in the classical dymamics literature—are first chosen to parametrize 
the configuration space. The sum of the potential and kinetic energies of the 
robot’s links are then expressed in terms of the generalized coordinates. These 
are then substituted into the Euler-Lagrange equations, which then lead to 
a set of second-order differentia equations for the dynamics, expressed in the 
chosen coordinates for the configuration space. 

In this chapter we examine both approaches to deriving a robot’s dynamic 
equations. Recursive algorithms for both the forward and inverse dynamics, as 
well as analytical formulations of the dynamic equations, are also presented. 

Chapter 9: Trajectory Generation 

What sets a robot apart from an automated machine is that it should be easily 
reprogrammable for different tasks. Different tasks require different motions, 
and it would be unreasonable to expect the user to specify the entire time-history 
of each joint for every task; clearly it would be desirable for the computer to 
“fill in the details” from a small set of task input data. 

This chapter is concerned with the automatic generation of joint trajectories 
from this set of task input data. Formally, a trajectory consists of a path , which 
is a purely geometric description of the sequence of configurations achieved by 
a robot, typically described by a curve in the joint configuration space. A 
trajectory also consists of a time scaling, which specifies the times at which 
those configurations are reached. 

Often the input task data is given in the form of an ordered set of joint 
values, called control points, together with a corresponding set of control times. 
Based on this data the trajectory generation algorithm produces a trajectory for 
each joint that satisfies various user-supplied conditions. In this chapter we will 
focus on three cases: (i) point-to-point straight line trajectories in both joint 
space and task space; (ii) smooth trajectories passing through a sequence of 
timed via points; (iii) minimum-time trajectories along specified paths. Finding 
paths that avoid collisions is the subject of the next chapter on motion planning. 

Chapter 10: Motion Planning 

This chapter addresses the problem of finding a collision-free motion for a robot 
through a cluttered workspace, while avoiding joint and torque limits, and other 
physical constraints imposed on the robot. The path planning problem is a 
subproblem of the general motion planning problem, and seeks to determine 
a collision-free path between a start and goal configuration, usually without 
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regard to the dynamics, the duration of the motion, or other constraints on the 
motion or control inputs. There is no single planner applicable to all motion 
planning problems. In this chapter we shall consider three basic approaches: 
grid methods, sampling methods, and methods based on virtual potential fields. 

Chapter 11: Robot Control 

A robot arm can exhibit a number of different behaviors depending on the task 
and its environment. It can act as a source of programmed motions for tasks 
such as moving an object from one place to another, or tracing a trajectory for 
manufacturing applications. It can act as a source of forces, for example when 
grinding or polishing a workpiece. In tasks such as writing on a chalkboard, it 
must control forces in some directions (the force pressing the chalk against the 
board) and motions in others (motion in the plane of the board). In certain 
applications, e.g., haptic displays, we may want the robot to act like a spring, 
damper, or mass, controlling its position, velocity, or acceleration in response 
to forces applied to it. 

In each of these cases, it is the job of the robot controller to convert the 
task specification to forces and torques at the actuators. Control strategies 
to achieve the behaviors described above are known as motion (or position) 
control, force control, hybrid motion-force control, and impedance con¬ 
trol. Which of these behaviors is appropriate depends on both the task and 
the environment. For example, a force control goal makes sense when the end- 
effector is in contact with something, but not when it is moving in free space. We 
also have a fundamental constraint imposed by mechanics, irrespective of the 
environment: the robot cannot independently control both motions and forces 
in the same direction. If the robot imposes a motion, then the environment 
determines the force, and vice versa. 

Most robots are driven by actuators that apply a force or torque to each 
joint. Hence, to precisely control a robot would require an understanding of 
the relationship between joint forces and torques and the motion of the robot; 
this is the domain of dynamics. Even for simple robots, however, the dynamic 
equations are usually very complex, Also, to accurately derive the dynamics 
requires, among other things, precise knowledge of the mass and inertia of each 
link, which may not be readily available. Even if they were, the dynamic equa¬ 
tions would still not reflect physical phenomena like friction, elasticity, backlash, 
and hysteresis. 

Most practical control schemes compensate for these errors by using feed¬ 
back. One effective method of industrial robot control is to neglect the robot’s 
dynamics, and instead model each actuator as a scalar second-order linear sys¬ 
tem. As such we first introduce basic concepts from linear control, and show 
how they can be used to effectively control complex multi-dof robots. 

This chapter also introduces some basic robot control techniques that as¬ 
sume a dynamic model of the robot is available; such feedforward control 
techniques use the dynamic model of the robot and its environment to deter¬ 
mine actuator control inputs that achieve the desired task. Because of modeling 
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and other errors, feedforward control is rarely used by itself, but is often used 
in conjunction with feedback control. After considering feedback and forward 
strategies for model-based motion control, we then examine force control, hybrid 
motion-force control, and impedance control. 

Chapter 12: Grasping and Manipulation 

The focus of the previous chapters has been mostly on the internal charac¬ 
terization of the robot—its kinematics and dynamics, as well as methods for 
motion planning and control. In this chapter we now explicitly consider physi¬ 
cal interactions between the robot and its environment. A fundamental problem 
addressed in this chapter is how to restrain a rigid body using a fixed number 
of point contacts. The first order of business is to characterize the nature of 
contacts between the robot and objects, or more generally, contact constraints 
between rigid bodies. As such it becomes necessary to consider friction. We 
examine the equations of motion for rigid body mechanics with friction, formu¬ 
late the general problem of manipulation plannning and grasping, and examine 
simplifications and assumptions that lead to certain basic solutions. 

Chapter 13: Wheeled Robots 

This chapter addresses the kinematics, motion planning, and control of wheeled 
robots that are subject to no-slip rolling constraints. Such constraints are funda¬ 
mentally different from the loop closure constraints found in closed chains—the 
former are holonomic, the latter nonholonomic- and as such we begin with 
a discussion of nonholonomic constraints. We then examine the kinematics 
of some popular wheeled robots: car-like, differential drive, Dubins, and om¬ 
nidirectional robots. The controllability problem of determining whether a 
wheeled robot is able to move from a given initial configuration to an arbitrary 
final configuration is then examined. The chapter concludes with a discussion 
of motion planning and control algorithms for wheeled robots, including the 
problem of characterizing and finding optimal paths, and feedback control of 
wheeled robots. 


Chapter 2 

Configuration Space 


A robot is mechanically constructed by connecting a set of bodies, called links, 
to each other using various types of joints. Actuators, such as electric motors, 
deliver forces or torques to the joints that cause the robot’s links to move. Usu¬ 
ally an end-effector, such as a gripper or hand for grasping and manipulating 
objects, is attached to a specific link. All of the robots considered in this book 
have links that can be modeled as rigid bodies. 

Perhaps the most fundamental question one can ask about a robot is, where 
is it? (In the sense of where the links of the robot are situated.) The answer 
is the robot’s configuration: a specification of the positions of all points of 
the robot. Since the robot’s links are rigid and of known shape, 1 only a few 
numbers are needed to represent the robot’s configuration. For example, the 
configuration of a door can be represented by a single number, the angle 8 
that the door rotates about its hinge. The configuration of a point lying on 
a plane can be described by two coordinates, ( x,y ). The configuration of a 
coin lying heads up on a flat table can be described by three coordinates: two 
coordinates (x, y) that specify the location of a particular point on the coin, and 
one coordinate 8 that specifies the coin’s orientation. (See Figure 2.1). 

The above coordinates all share the common feature of taking values over 
a continuous range of real numbers. The smallest number of real-valued coor¬ 
dinates needed to represent a robot’s configuration is its degrees of freedom 
(dof). In the example above, the door (regarded as a robot) has one degree of 
freedom. The coin lying heads up on a table has three degrees of freedom. Even 
if the coin could he either heads up or tails up, its configuration space still would 
have only three degrees of freedom; introducing a fourth variable to represent 
which side of the coin is facing up, this variable would then take values in the 
discrete set {heads, tails}, and not over a continuous range of real values like 
the other three coordinates ( x,y,8 ). 

Definition 2.1. The configuration of a robot is a complete specification of 
the positions of every point of the robot. The minimum number n of real- 

1 Compare with trying to represent the configuration of a soft object like a pillow. 
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Figure 2.1: (a) The configuration of a door is described by its angle 0. (b) The 
configuration of a point in a plane is described by coordinates (x,y). (c) The 
configuration of a coin on a table is described by ( x , y, 6). 


valued coordinates needed to represent the configuration is the degrees of 
freedom (dof) of the robot. The n-dimensional space containing all possible 
configurations of the robot is called the configuration space (or C-space). 

In this chapter we study the C-space and degrees of freedom of general 
robots. Since our robots are constructed of rigid links, we first examine the 
degrees of freedom of a single rigid body, followed by the degrees of freedom of 
general multi-link robots. We then study the shape (or topology) and geometry 
of C-spaces and their mathematical representation. The chapter concludes with 
a discussion of the C-space of a robot’s end-effector, or its task space. In the 
next chapter we study in more detail the various mathematical representations 
for the C-space of a single rigid body. 

2.1 Degrees of Freedom of a Rigid Body 

Continuing with the example of the coin lying on the table, choose three points 
A , B, and C on the coin (Figure 2.2(a)). Once a coordinate frame x-y is attached 
to the plane, 2 the positions of these points in the plane are written ( xa,Va ), 
(xb,Vb), and (x c, yc )• If these points could be placed independently anywhere 
in the plane, the coin would have six degrees of freedom—two for each of the 
three points. However, according to the definition of a rigid body, the distance 
between point A and point B , denoted d(A,B), is always constant regardless of 
where the coin is. Similarly, the distances d{B , C) and d(A, C ) must be constant. 
The following equality constraints on the coordinates (xa,Va), ( xb,Vb ), and 

2 The unit axes of coordinate frames are written with a hat, indicating they are unit vectors, 
in a non-italic font, e.g., x, y, and z. 
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Figure 2.2: (a) Choosing three points fixed to the coin, (b) Once the location 
of A is chosen, B must lie on a circle of radius dAB centered at A. Once the 
location of B is chosen, C must lie at the intersection of circles centered at 
A and B. Only one of these two intersections corresponds to the “heads up” 
configuration, (c) The configuration of a coin in three-dimensional space is given 
by the three coordinates of A, two angles to the point B on the sphere of radius 
dAB centered at A, and one angle to the point C on the circle defined by the 
intersection of the a sphere centered at A and a sphere centered at B. 




(xc,yc) must therefore always be satisfied: 

d(A, B) = yj(x A - x B ) 2 + {ua - Vb) 2 = d A B 

d(B , C) = \J(x B ~ x c ) 2 + {vb - yc ) 2 = d B c 

d(A , C) = \J (xa - x c ) 2 + (va ~ yc) 2 = d A c- 

To determine the number of degrees of freedom of the coin, first choose the 
position of point A in the plane (Figure 2.2(b)). We may choose it to be anything 
we want, so we have two degrees of freedom to specify, ( xa,Va )■ Once (xa,Va) 
is specified, the constraint d(A,B) = dAB restricts the choice of (xb,Vb) to 

those points on the circle of radius dAB centered at A. A point on this circle 

can be specified by a single parameter, e.g., the angle specifying the location of 
B on the circle centered at A; let’s call this angle <f>AB , and define it to be the 
angle that the vector A11 makes with the x-axis. 

Once we have chosen the location of point B, there are only two possible 
locations for C: at the intersections of the circle of radius centered at A, 
and the circle of radius dsc centered at B (Figure 2.2(b)). These two solutions 
correspond to heads or tails. In other words, once we have placed A and B and 
chosen heads or tails, the two constraints d(A,C) = dAC and d(B,C) = dsc 
eliminate the two apparent freedoms provided by ( xc,yc ), and the location of 
C is fixed. The coin has exactly three degrees of freedom in the plane, which 
can be specified by (xa, Va, 4>ab)■ 

Suppose we were to choose an additional point D on the coin. This new 
point introduces three additional constraints: d(A,D ) = c?ad, d(B,D) = 
and d{C,D) = dcD- One of these constraints is redundant, i.e., it provides no 
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new information; only two of the three constraints are independent. The two 
freedoms apparently introduced by the coordinates (xd,Vd) are then immedi¬ 
ately eliminated by these two independent constraints. The same would hold for 
any other newly chosen point on the coin, so that there is no need to consider 
additional points. 

We have been applying the following general rule for determining the number 
of degrees of freedom of a system: 

Degrees of freedom = (Sum of freedoms of the points) — 

(Number of independent constraints). (2-1) 

This rule can also be expressed in terms of the number of variables and inde¬ 
pendent equations that describe the system: 

Degrees of freedom = (Number of variables) — 

(Number of independent equations). (2-2) 

This general rule can also be used to determine the number of freedoms of 
a rigid body in three dimensions. For example, assume our coin is no longer 
confined to the table (Figure 2.2(c)). The coordinates for the three points A, B, 
and C are now given by (x A , y A ,z A ), (x B , Vb,zb), and (x c , He Ac), respectively. 
Point A can be placed freely (three degrees of freedom). The location of point B 
is subject to the constraint d(A, B) = d AB , meaning it must lie on the sphere of 
radius d AB centered at A. Thus we have 3—1 = 2 freedoms to specify, which can 
be expressed as the latitude and longitude for the point on the sphere. Finally, 
the location of point C must lie at the intersection of spheres centered at A and 
B of radius d A c and d B c, respectively. In the general case the intersection of 
two spheres is a circle, and the location of point C can be described by an angle 
that parametrizes this circle. Point C therefore adds 3—2 = 1 freedom. Once 
the position of point C is chosen, the coin is fixed in space. 

In summary, a rigid body in three-dimensional space has six freedoms, which 
can be described by the three coordinates parametrizing point A, the two angles 
parametrizing point B , and one angle parametrizing point C. Other represen¬ 
tations for the configuration of a rigid body are discussed in Chapter 3. 

We have just established that a rigid body moving in three-dimensional 
space, which we call a spatial rigid body, has six degrees of freedom. Similarly, 
a rigid body moving in a two-dimensional plane, which we henceforth call a 
planar rigid body, has three degrees of freedom. This latter result can also 
be obtained by considering the planar rigid body to be a spatial rigid body with 
six degrees of freedom, but with the three independent constraints z A = z B = 
zc = 0 . 

Since our robots are constructed of rigid bodies, Equation (2.1) can be ex¬ 
pressed as follows: 

Degrees of freedom = (Sum of freedoms of the bodies) — 

(Number of independent constraints). (2-3) 


2.2. Degrees of Freedom of a Robot 


13 


Equation (2.3) forms the basis for determining the degrees of freedom of general 
robots, which is the topic of the next section. 

2.2 Degrees of Freedom of a Robot 

Consider once again the door example of Figure 2.1(a), consisting of a single 
rigid body connected to the wall by a hinge joint. From the previous section we 
know that the door has only one degree of freedom, conveniently represented by 
the hinge joint angle 9. Without the hinge joint, the door is free to move in three- 
dimensional space and has six degrees of freedom. By connecting the door to the 
wall via the hinge joint, five independent constraints are imposed on the motion 
of the door, leaving only one independent coordinate (9). Alternatively, the 
door can be viewed from above and regarded as a planar body, which has three 
degrees of freedom. The hinge joint then imposes two independent constraints, 
again leaving only one independent coordinate (9). Its C-space is represented 
by some range in the interval [0, 27 t) over which 9 is allowed to vary. 

In both cases the joints have the effect of constraining the motion of the 
rigid body, and thus reducing the overall degrees of freedom. This observation 
suggests a formula for determining the number of degrees of freedom of a robot, 
simply by counting the number of rigid bodies and joints. In this section we 
derive precisely such a formula, called Griibler’s formula, for determining the 
number of degrees of freedom of planar and spatial robots. 

2.2.1 Robot Joints 

Figure 2.3 illustrates the basic joints found in typical robots. Every joint con¬ 
nects exactly two links; joint that simultaneously connect three or more links 
are not allowed. The revolute joint (R), also called a hinge joint, allows for 
rotational motion about the joint axis. The prismatic joint (P), also called a 
sliding or linear joint, allows for translational (or rectilinear) motion along the 
direction of the joint axis. The screw joint (H), also called a helical joint, allows 
simultaneous rotation and translation about a screw axis. Revolute, prismatic, 
and screw joints all have one degree of freedom. 

Joints can also have multiple degrees of freedom. The cylindrical joint 
(C) is a two-dof joint that allows for independent translations and rotations 
about a single fixed joint axis. The universal joint (U) is another two-dof 
joint that consists of a pair of revolute joints arranged so that their joint axes 
are orthogonal. The spherical joint (S), also called a ball-and-socket joint, 
has three degrees of freedom and functions much like our shoulder joint. 

A joint can be viewed as providing freedoms to allow one rigid body to 
move relative to another. It can also be viewed as providing constraints on the 
possible motions of the two rigid bodies it connects. For example, a revolute 
joint can be viewed as allowing one freedom of motion between two rigid bodies 
in space, or it can be viewed as providing five constraints on the motion of one 
rigid body relative to the other. Generalizing, the number of degrees of freedom 
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Figure 2.3: Typical robot joints. 


Joint type 

dof / 

Constraints c 
between two 
planar 
rigid bodies 

Constraints c 
between two 
spatial 
rigid bodies 

Revolute (R) 

1 

2 

5 

Prismatic (P) 

1 

2 

5 

Screw (H) 

1 

N/A 

5 

Cylindrical (C) 

2 

N/A 

4 

Universal (U) 

2 

N/A 

4 

Spherical (S) 

3 

N/A 

3 


Table 2.1: The number of degrees of freedom and constraints provided by com¬ 
mon joints. 


of a rigid body (three for planar bodies and six for spatial bodies) minus the 
number of constraints provided by a joint must equal the number of freedoms 
provided by the joint. 

The freedoms and constraints provided by the various joint types are sum¬ 
marized in Table 2.1. 

2.2.2 Griibler’s Formula 

The number of degrees of freedom of a mechanism with links and joints can be 
calculated using Griibler’s formula, which is an expression of Equation (2.3). 

Proposition 2.1. Consider a mechanism consisting of N links, where ground 
is also regarded as a link. Let J be the number of joints, m be the number of 
degrees of freedom of a rigid body (m = 3 for planar mechanisms and m = 6 for 























2.2. Degrees of Freedom of a Robot 


15 



Figure 2.4: 


(a) Four-bar linkage, (b) Slider-crank mechanism. 


spatial mechanisms), fi be the number of freedoms provided by joint i, and Ci be 
the number of constraints provided by joint i (it follows that fi + Ci = m for all 
i). Then Griibler’s formula for the degrees of freedom (dof) of the robot is 

J 

dof = m(N — i) — y g 

rigid body freedoms v > 

joint constraints 
J 

= m{N - 1) - y (to - fi) 

2—1 

J 

= m { N - l - J ) + Y , fi - (2-4) 

i= 1 

This formula only holds if all joint constraints are independent. If they are not 
independent, then the formula provides a lower bound on the number of degrees 
of freedom. 

Below we apply Griibler’s formula to several planar and spatial mechanisms. 
We distinguish between two types of mechanisms: open-chain mechanisms 
(also known as serial mechanisms) and closed-chain mechanisms. A 
closed-chain mechanism is any mechanism that has a closed loop. A person 
standing with both feet on the ground is an example of a closed-chain mech¬ 
anism, since a closed loop is traced from the ground, through the right leg, 
through the waist, through the left leg, and back to the ground (recall that 
ground itself is a link). An open-chain mechanism is any mechanism without a 
closed loop; an example is your arm when your hand is allowed to move freely 
in space. 

Example 2.1. Four-bar linkage and slider-crank mechanism 

The planar four-bar linkage shown in Figure 2.4(a) consists of four links (one of 
them ground) arranged in a single closed loop and connected by four revolute 
joints. Since all the links are confined to move in the same plane, m = 3. 
Subsituting N = 4, J = 4, and /» = 1, i = 1,..., 4, into Grubler’s formula, we 
see that the four-bar linkage has one degree of freedom. 

The slider-crank closed-chain mechanism of Figure 2.4(b) can be analyzed in 
two ways: (i) the mechanism consists of three revolute joints and one prismatic 
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(c) (d) 


Figure 2.5: (a) fc-link planar serial chain, (b) Five-bar planar linkage, (c) 
Stephenson six-bar linkage, (d) Watt six-bar linkage. 


joint (J = 4, and each fi = 1) and four links (N = 4, including the ground 
link), or (ii) the mechanism consists of two revolute joints (fi = 1) and one RP 
joint (the RP joint is a concatenation of a revolute and prismatic joint, so that 
fi = 2) and three links (N = 3; remember that each joint connects precisely 
two bodies). In both cases the mechanism has one degree of freedom. 

Example 2.2. Some classical planar mechanisms 

Let us now apply Griibler’s formula to several classical planar mechanisms. The 
fc-link planar serial chain of revolute joints in Figure 2.5(a) (called a kR robot 
for its k revolute joints) has N = k + 1 (k links plus ground) and J = k, and 
since all the joints are revolute, each fi = 1. Therefore, 

dof = 3 ((k +1) — 1 — k) + k = k 

as expected. For the planar five-bar linkage of Figure 2.5(b), N = 5 (four links 
plus ground), J = 5, and since all joints are revolute, each fi = 1. Therefore, 

dof = 3(5 — 1 — 5) + 5 = 2. 

For the Stephenson six-bar linkage of Figure 2.5(c), we have N = 6, J = 7, and 
fi = 1 for all i, so that 


dof = 3(6 — 1 — 7) + 7 = 1. 
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Figure 2.6: A planar mechanism with two overlapping joints. 

Finally, for the Watt six-bar linkage of Figure 2.5(d), we have N = 6, J = 7, 
and fi = 1 for all i. so that like the Stephenson six-bar linkage, 

dof = 3(6 — 1 — 7) + 7 = 1. 

Example 2.3. A planar mechanism with overlapping joints 

The planar mechanism illustrated in Figure 2.6 has three links that meet at a 
single point on the right of the large link. Recalling that a joint by definition 
connects exactly two links, the joint at this point of intersection should not 
be regarded as a single revolute joint. Rather, it is correctly interpreted as 
two revolute joints overlapping each other. Again, there is more than one way 
to derive the number of degrees of freedom of this mechanism using Griibler’s 
formula: (i) The mechanism consists of eight links (N = 8), eight revolute joints, 
and one prismatic joint. Substituting into Griibler’s formula, 

dof = 3(8 — 1 — 9) + 9(1) =3. 

(ii) Alternatively, the lower-right revolute-prismatic joint pair can be regarded 
as a single two-dof joint. In this case the number of links is N = 7, with seven 
revolute joints, and a single two-dof revolute-prismatic pair. Substituting into 
Griibler’s formula, 


dof = 3(7 - 1 - 8) + 7(1) + 1(2) = 3. 

Example 2.4. Griibler’s formula and singularities 

For the parallelogram linkage of Figure 2.7(a), N = 5, J = 6, and _/)= 1 
for each joint. From Griibler’s formula, the degrees of freedom is given by 
3(5 — 1 — 6) + 6 = 0. A mechanism with zero degrees of freedom is by definition 
a rigid structure. However, if the three parallel links are of the same length, 
and the two horizontal rows of joints are also collinear as implied by the figure, 
the mechanism can in fact move with one degree of freedom. 

A similar situation occurs for the two-dof planar five-bar linkage of Fig¬ 
ure 2.7(b). If the two joints connected to ground are locked at some fixed angle, 
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(a) (b) 


Figure 2.7: (a) A parallelogram linkage; (b) The five-bar linkage in a regular 
and singular configuration. 



the five-bar linkage should then become a rigid structure. However, if the two 
middle links are of equal length and overlap each other as illustrated in the 
figure, then these overlapping links can rotate freely about the two overlapping 
joints. Of course, the link lengths of the five-bar linkage must meet certain 
specifications in order for such a configuration to even be possible. Also note 
that if a different pair of joints is locked in place, the mechanism does become 
a rigid structure as expected. 

Griibler’s formula provides a lower bound on the degrees of freedom for 
singular cases like those just described. Configuration space singularities arising 
in closed chains are discussed in Chapter 7. 

Example 2.5. Delta robot 

The Delta robot of Figure 2.8 consists of two platforms—the lower one mobile, 
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the upper one stationary—connected by three legs: each leg consists of an RR 
serial chain connected to a closed-loop parallelogram linkage. Naively applying 
the spatial version of Grubler’s formula yields N = 17, J = 21 (all revolute 
joints), and dof = 17(17 — 1 — 21) + 21 = —9, which would imply that the 
mechanism is over const rained and therefore a rigid structure. However, each 
parallelogram linkage has one degree of freedom of motion, so that each leg 
of the Delta robot is kinematically equivalent to an RUU chain. In this case 
N = 8, J = 9 (six U joints and three R joints), and 

dof = 6(8 - 1 - 9) + 3(1) + 6(2) = 3. 

The Delta robot has three degrees of freedom. The moving platform in fact 
always remains parallel to the fixed platform and in the same orientation, so 
that the Delta robot in effect acts as a Cartesian positioning device. 

Example 2.6. Stewart-Gough Platform 

The Stewart-Gough platform of Figure 1.1(b) consists of two platforms—the 
lower one stationary and regarded as ground, the upper one mobile—connected 
by six universal-prismatic-spherical (UPS) serial chains. The total number of 
links is fourteen (TV = 14). There are six universal joints (each with two degrees 
of freedom, /) = 2), six prismatic joints (each with a single degree of freedom, 
fi = 1), and six spherical joints (each with three degrees of freedom, fi = 3). 
The total number of joints is 18. Substituting these values into Griibler’s formula 
with m = 6, 


dof = 6(14 - 1 - 18) + 6(1) + 6(2) + 6(3) = 6. 

In some versions of the Stewart-Gough platform the six universal joints are 
replaced by spherical joints. By Grubler’s formula this mechanism would have 
twelve degrees of freedom; replacing each universal joint by a spherical joint 
introduces an extra degree of freedom in each leg, allowing torsional rotations 
about the leg axis. Note however that this torsional rotation has no effect on 
the motion of the mobile platform. 

The Stewart-Gough platform is a popular choice for car and airplane cockpit 
simulators, as the platform moves with the full six degrees of freedom of motion 
of a rigid body. The parallel structure means that each leg only needs to support 
a fraction of the weight of the payload. On the other hand, the parallel design 
also limits the range of translational and rotational motion of the platform. 

2.3 Configuration Space: Topology and Represen¬ 
tation 

2.3.1 Configuration Space Topology 

Until now we have been focusing on one important aspect of a robot’s C-space— 
its dimension, or the number of degrees of freedom. However, the shape of the 
space is also important. 
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Figure 2.9: An open interval of the real line, denoted (a, b), can be deformed 
to an open semicircle. This open semicircle can then be deformed to the real 
line by the mapping illustrated: beginning from a point at the center of the 
semicircle, draw a ray that intersects the semicircle and then a line above the 
semicircle. These rays show that every point of the semicircle can be stretched 
to exactly one point on the line, and vice-versa. Thus an open interval can be 
continuously deformed to a line, so an open interval and a line are topologically 
equivalent. 


Consider a point moving on the surface of a sphere. The point’s C-space is 
two-dimensional, as the configuration can be described by two coordinates, e.g., 
latitude and longitude. As another example, a point moving on a plane also 
has a two-dimensional C-space, with coordinates (x, y). While both a plane and 
the surface of a sphere are two-dimensional, clearly they do not have the same 
shape—the plane extends infinitely while the sphere wraps around. 

On the other hand, a larger sphere has the same shape as the original sphere, 
in that it wraps around in the same way. Only its size is different. For that mat¬ 
ter, an oval-shaped American football also wraps around similarly to a sphere. 
The only difference between a football and a sphere is that the football has been 
stretched in one direction. 

The idea that the two-dimensional surfaces of a small sphere, a large sphere, 
and a football all have the same kind of shape, which is different from the shape 
of a plane, is expressed by the topology of the surfaces. We do not attempt a 
rigorous treatment in this book 3 , but we say that two spaces are topologically 
equivalent if one can be continuously deformed into the other without cutting 
or gluing. A sphere can be deformed into a football simply by stretching, without 
cutting or gluing, so those two spaces are topologically equivalent. You cannot 
turn a sphere into a plane without cutting it, however, so a sphere and a plane 
are not topologically equivalent. 

Topologically distinct one-dimensional spaces include the circle, the line, and 
a closed interval of the line. The circle is written mathematically as S' or S' 1 , 
i.e., a one-dimensional “sphere.” The line can be written E or E 1 , indicating a 
one-dimensional Euclidean (or “flat”) space. Since a point in E 1 is so commonly 
represented by a real number (after choosing an origin and a length scale), it is 
often written R or R 1 instead. A closed interval of the line, which contains its 
endpoints, can be written [a, b] C R 1 . (An open interval (a, 6) does not include 
the endpoints a and b and is topologically equivalent to a line, since the open 
interval can be stretched to a line, as shown in Figure 2.9. A closed interval is 
not topologically equivalent to a line, since a line does not contain endpoints.) 


3 For those familiar with concepts in topology, all spaces we consider can be viewed as 
embedded in a higher-dimensional Euclidean space, inheriting the Euclidean topology of that 
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In higher dimensions, R" is the n-dimensional Euclidean space and S n is the 
n-dimensional surface of a sphere in (n + l)-dimensional space. For example, 
S 2 is the two-dimensional surface of a sphere in three-dimensional space. 

Note that the topology of a space is a fundamental property of the space 
itself, and it is independent of how we choose to use coordinates to represent 
points in the space. For example, to represent a point on a circle, we could refer 
to the point by the angle 6 from the center of the circle to the point, relative 
to a chosen zero angle. Or we could choose a reference frame with its origin at 
the center of the circle and represent the point by the two coordinates (x,y), 
subject to the constraint x 2 + y 2 = 1. No matter our choice of coordinates, the 
space itself does not change. 

Some C-spaces can be expressed as the Cartesian product of two or more 
spaces of lower dimension; that is, points in such a C-space can be represented 
as the union of the representation of points in the lower-dimensional spaces. For 
example: 

• The C-space of a rigid body in the plane can be written as R 2 x 5 1 , 
since the configuration can be represented as the concatenation of the 
coordinates (x,y) representing R 2 and an angle 6 representing S 1 . 

• The C-space of a PR robot arm can be written R 1 x S' 1 . (Typically we will 
not worry about joint limits when expressing the topology of the C-space.) 

• The C-space of a 2R robot arm can be written as S 1 x S 1 = T 2 , where 
T" is the n-dimensional surface of a torus in an (n +1)-dimensional space. 
(See Table 2.2.) Note that S 1 x S 1 x ... x S 1 (n copies of S 1 ) is equal to 
T n , not S n ; for example, a sphere S 2 is not topologically equivalent to a 
torus T 2 . 

• The C-space of a planar rigid body (e.g., the chassis of a mobile robot) 
with a 2R robot arm can be written as R 2 x S 1 x T 2 = R 2 x T 3 . 

• As we saw in Section 2.1 when we counted the degrees of freedom of a 
rigid body in three dimensions, the configuration of a rigid body can be 
described by a point in R , plus a point on a two-dimensional sphere 
S' 2 , plus a point on a one-dimensional circle S 1 , for a total C-space of 
R 3 x S 2 x S 1 . 


2.3.2 Configuration Space Representation 

To perform computations, we must have a numerical representation of the space, 
consisting of a set of real numbers. We are familiar with this idea from linear 
algebra—a vector is a natural way to represent a point in a Euclidean space. 
It is important to keep in mind that the representation of a space involves a 
choice, and therefore it is not as fundamental as the topology of the space itself, 


space. 
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system 


C-space 


sample representation 



point on a plane 


E 2 



2R robot arm 


2-torus T 2 = S 1 x S 


[0,2tt) x [0, 2 tt) 




2jt — ^ 

0 -» 



rotating sliding knob 


cylinder R 1 x S' 1 


R 1 x [0, 2 t r) 


Table 2.2: Four topologically different two-dimensional C-spaces and exam¬ 
ple coordinate representations. In the latitude-longitude representation of the 
sphere, the latitudes —90° and 90° each correspond to a single point (the South 
Pole and the North Pole, respectively), and the longitude parameter wraps 
around at 180° and —180°: the edges with the arrows are glued together. Sim¬ 
ilarly, the coordinate representations of the torus and cylinder wrap around at 
the edges marked with identical arrows. To turn the torus into its coordinate 
representation (a subset of R 2 ), the torus can be cut along the small circle 
shown (representing the range of angles 62 of the second joint while Q\ = 0) and 
straightened out to make a cylinder, then cut along the length of the cylinder 
(representing the range of angles of the first joint while 02 = 0) and flattened. 


which is independent of the representation. For example, the same point in a 
3D space can have different coordinate representations depending on the choice 
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of the reference frame (the origin and the direction of the coordinate axes) and 
the choice of length scale, but the topology of the underlying space is the same 
regardless of our choice. 

While it is natural to choose a reference frame and length scale and use 
a vector to represent points in a Euclidean space, representing a point on a 
curved space, like a sphere, is less obvious. One solution for a sphere is to use 
latitude and longitude coordinates. A choice of n coordinates, or parameters, 
to represent an n-dimensional space is called an explicit parametrization of 
the space. The explicit parametrization is valid for a particular range of the 
parameters (e.g., [—90°,90°] for latitude and [—180°, 180°) for longitude for a 
sphere, where, on Earth, negative values correspond to “South” and “West,” 
respectively). 

The latitude-longitude representatation of a sphere is dissatisfying if you 
are walking near the North Pole (latitude equals 90°) or South Pole (latitude 
equals —90°), where taking a very small step can result in a large change in the 
coordinates. The North and South Poles are singularities of the representation, 
and the existence of singularities is a result of the fact that a sphere does not 
have the same topology as a plane, i.e., the space of the two real numbers 
that we have chosen to represent the sphere (latitude and longitude). The 
location of these singularities has nothing to do with the sphere itself, which 
looks the same everywhere, and everything to do with the chosen representation 
of it. Singularities of the parametrization are particularly problematic when 
representing velocities as the time rate of change of coordinates, since these 
representations may tend to infinity near singularities even if the point on the 
sphere is moving at a constant speed yj x 2 + y 2 + z 2 (if you had represented the 
point as (x,y,z) instead). 

If you assume that the configuration never approaches a singularity of the 
representation, you can ignore this issue. If you cannot make this assumption, 
there are two ways to overcome the problem: 

• Define more than one coordinate chart on the space, where each co¬ 
ordinate chart is an explicit parametrization covering only a portion of 
the space. Within each chart, there is no singularity. For example, 
we could define two coordinate charts on the sphere: the usual latitude 
(f> € [—90°,90°] and longitude ip G [—180°, 180°), and alternative coordi¬ 
nates (<//, ip') in a rotated coordinate frame, where the alternative latitude 
<jj is 90° at the “East Pole” and —90° at the “West Pole.” Then the 
first coordinate chart can be used when —90° + e < <j> < 90° — e, for 
some small e > 0, and the second coordinate chart can be used when 
—90° + e <(/)'< 90° - e. 

If we define a set of singularity-free coordinate charts that overlap each 
other and cover the entire space, like the two charts above, the charts are 
said to form an atlas of the space, much like an atlas of the Earth consists 
of several maps that together cover the Earth. An advantage of using 
an atlas of coordinate charts is that the representation always uses the 
minimum number of numbers. A disadvantage is the extra bookkeeping 
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required to switch the representation between coordinate charts to avoid 
singularities. (Note that Euclidean spaces can be covered by a single 
coordinate chart without singularities.) 

• Instead of using an explicit parametrization, use an implicit represen¬ 
tation of the space. An implicit representation views the n-dimensional 
space as embedded in a Euclidean space of more than n dimensions, just 
like a two-dimensional unit sphere can be viewed as a surface embed¬ 
ded in a three-dimensional Euclidean space. An implicit representation 
uses the coordinates of the higher-dimensional space (e.g., (x,y,z) in the 
three-dimensional space), but subjects these coordinates to constraints 
that reduce the number of degrees of freedom (e.g., x 2 + y 2 + z 2 = 1 for 
the unit sphere). 

A disadvantage of this approach is that the representation has more num¬ 
bers than the number of degrees of freedom. An advantage is that there are 
no singularities in the representation—a point moving smoothly around 
the sphere is represented by a smoothly changing (x,y,z), even at the 
North and South Poles. A single representation is used for the whole 
sphere; multiple coordinate charts are not needed. 

Another advantage is that while it may be very difficult to construct an 
explicit parametrization, or atlas, for a closed-chain mechanism, it is easy 
to find an implicit representation: the set of all joint coordinates subject 
to the loop-closure equations that define the closed loops (Section 2.4). 

We use implicit representations throughout the book, beginning in the 
next chapter. In particular, we use nine numbers, subject to six con¬ 
straints, to represent the three orientation freedoms of a rigid body in 
space. This is called a rotation matrix. In addition to being singularity-free 
(unlike three-parameter representations such as roll-pitch-yaw angles 4 ), 
the rotation matrix representation has the benefit of allowing us to use 
linear algebra to perform computations such as (1) rotating a rigid body 
or (2) changing the reference frame in which the orientation of a rigid 
body is expressed. 5 

In summary, the non-Euclidean shape of many C-spaces motivates the use 
of implicit representations of C-space throughout this book. We return to this 
topic in the next chapter. 
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Figure 2.10: The four-bar linkage. 

2.4 Configuration and Velocity Constraints 

For robots containing one or more closed loops, usually an implicit represen¬ 
tation is more easily obtained than an explicit parametrization. For example, 
consider the planar four-bar linkage of Figure 2.10, which has one degree of free¬ 
dom. The fact that the four links always form a closed loop can be expressed 
in the form of the following three equations: 

L\ cos 0 \ H- L 2 cos($i + #2) + ■ • ■ + T4 cos($i T ... T $4) = 0 

L\ sin 0 \ -(- L2 sin(6*i -f- O2 ) T ... T T4 sin($i T ... T $4) = 0 

6 >i + e 2 + e 3 + di — 2tt = 0. 

These equations are obtained by viewing the four-bar linkage as a serial chain 
with four revolute joints, in which (i) the tip of link L 4 always coincides with 
the origin and (ii) the orientation of link T 4 is always horizontal. 

These equations are sometimes referred to as loop-closure equations. For 
the four-bar linkage they are given by a set of three equations in four unknowns. 
The set of all solutions forms a one-dimensional curve in the four-dimensional 
joint space and constitutes the C-space. 

For general robots containing one or more closed loops, the configuration 
space can be implicitly represented by 9 = (9 1 ,..., 9 n ) £ R” and loop-closure 
equations of the form 


9(0) 


9l(0\, ■ ■ ■ , On) 

9k(0l, ■ ■ ■ , 0 n ) 


= 0 , 


(2.5) 


4 Roll-pitch-yaw angles and Euler angles use three parameters for the space of rotations 
5 2 x 5 1 (two for S 2 and one for S 1 ), and therefore are subject to singularities as discussed 
above. 

5 Another singularity-free implicit representation of orientations, the unit quaternion, uses 
only four numbers subject to the constraint that the four-vector be unit length. In fact, this 
representation is a double covering of the set of orientations: for every orientation, there are 
two unit quaternions. 
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where g : R” —> R fc is a set of k independent equations, with k < n. Such 
constraints are known as holonomic constraints, constraints that reduce the 
dimension of the C-space. 6 The C-space can be viewed as a surface of dimension 
n — k (assuming all constraints are independent) embedded in R". 

Suppose a closed-chain robot with loop-closure equations g(9) = 0, g : R" —> 
R fc , is in motion, following the time trajectory 9(t). Differentiating both sides 
of g{9(t)) = 0 with respect to t , we obtain 






dg i 




few 






feWfln J 

“ 01 


d &) J L 




= o 


= o 


= 0 


= 0 . 


( 2 . 6 ) 


Here 9i denotes the time derivative of 9i with respect to time f, [6) £ R lx ", 
and 9,9 £ R n . From the above we see that the joint velocity vector 9 € R" 
cannot be arbitrary, but must always satisfy 


s»= a 


(2.7) 


These constraints can be written in the form 


A(9)9 = 0, 


( 2 . 8 ) 


where A(9) £ R kxn . Velocity constraints of this form are called PfafRan con¬ 
straints. For the case of A(9) = || (0), one could regard g{9) as being the 
“integral” of A(6 ); for this reason, holonomic constraints of the form g(9) = 0 
are also called integrable constraints —the velocity constraints that they im¬ 
ply can be integrated to give equivalent configuration constraints. 

We now consider another class of Pfaffian constraints that is fundamentally 
different from the holonomic type. To illustrate with a concrete example, con¬ 
sider an upright coin of radius r rolling on the plane as shown in Figure 2.11. 
The configuration of the coin is given by the contact point ( x,y ) on the plane, 
the steering angle <j>, and the angle of rotation (see Figure 2.11). The C-space of 
the coin is therefore R 2 x T 2 , where T 2 is the two-dimensional torus parametrized 
by the angles (j) and 9. This C-space is four-dimensional. 

6 Viewing a rigid body as a collection of points, the distance constraints between the points, 
as we saw earlier, can be viewed as holonomic constraints. 
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We now express, in mathematical form, the fact that the coin rolls without 
slipping. The coin must always roll in the direction indicated by (cos <j>, sin <fi), 
with forward speed r9: 

x 

y 


= rd 


cos ft 
sin d> 


(2.9) 


Collecting the four C-space coordinates into a single vector q = ( 91 , 92,93, 94) = 
(x, y , <fi, 9) £ I 2 x T 2 , the above no-slip rolling constraint can then be expressed 
in the form 


1 0 0 — r cos 93 
0 10 —r sin 93 


9 = 0. 


( 2 . 10 ) 


These are Pfafhan constraints of the form A(q)q = 0, ^( 9 ) £ R 2x4 . 

These constraints are not integrable; that is, for the A(q) given in (2.10), 
there does not exist any differentiable g : R 4 —> R 2 such that = A(q). To see 
why, there would have to exist a differentiable 91 ( 9 ) that satisfied the following 
four equalities: 


dm 

d qi 

091 

dq 2 

ogi 

dq 3 
og 1 

dqi 


1 —t 91 ( 9 ) 

0 —> 91 ( 9 ) 

0 —> 9i(9) 

~r cos 93 — X 91 ( 9 ) 


9i + ^i(92,93,94) 
^ 2 ( 91 , 93 , 94 ) 

^3(91,92,94) 

—rg 4 cos93 -1-/14(91,92,93), 


for some hi, i = 1,... ,4, differentiable in each of its variables. By inspection 
it should be clear that no such 91 ( 9 ) exists. Similarly, it can be shown that 
92 ( 9 ) does not exist, so that the constraint (2.10) is nonintegrable. A Pfafhan 
constraint that is nonintegrable is called a nonholonomic constraint. Such 
constraints reduce the dimension of the feasible velocities of the system, but 
do not reduce the dimension of the reachable C-space. The rolling coin can 
reach any point in its four-dimensional C-space despite the two constraints on 
its velocity . 7 See Exercise 28. 

7 Some texts define the number of degrees of freedom of a system to be the dimension of 
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Figure 2.12: Examples of workspaces for various robots: (a) a planar 2R open 
chain; (b) a planar 3R open chain; (c) a spherical 2R open chain; (d) a 3R 
orienting mechanism. 


Nonholonomic constraints arise in a number of robotics contexts that involve 
conservation of momentum and rolling without slipping, e.g., wheeled vehicle 
kinematics and grasp contact kinematics. We examine nonholonomic constraints 
in greater detail in the later chapter on wheeled robots. 

2.5 Task Space and Workspace 

We introduce two more concepts relating to the configuration of a robot: the 
task space and the workspace. Both relate to the configuration of the end- 
effector of a robot, not the configuration of the entire robot. 

The task space is a space in which the robot’s task can be naturally ex¬ 
pressed. For example, if the robot’s task is to plot with a pen on a piece of paper, 
the task space would be R 2 . If the task is to manipulate a rigid body, a natural 
representation of the task space is the C-space of a rigid body, representing the 
position and orientation of a frame attached to the robot’s end-effector. This is 


the feasible velocities, e.g., two for the rolling coin. We uniformly refer to the dimension of 
the C-space as the number of degrees of freedom. 
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the default representation of task space. The decision of how to define the task 
space is driven by the task, independent of the robot. 

The workspace is a specification of the configurations the end-effector of 
the robot can reach. The definition of the workspace is primarily driven by the 
robot’s structure, independent of the task. 

Both the task space and the workspace involve a choice by the user; in 
particular, the user may decide that some freedoms of the end-effector (e.g., its 
orientation) do not need to be represented. 

The task space and the workspace are distinct from the robot’s C-space. A 
point in the task space or the workspace may be achievable by more than one 
robot configuration, meaning that the point is not a full specification of the 
robot’s configuration. For example, for an open-chain robot with seven joints, 
the six-dof position and orientation of its end-effector does not fully specify the 
robot’s configuration. 

Some points in task space may not be reachable at all by the robot, e.g., a 
point on a chalkboard that the robot cannot reach. By definition, all points in 
the workspace are reachable by at least one configuration of the robot. 

Two mechanisms with different C-spaces may have the same workspace. For 
example, considering the end-effector to be the Cartesian tip of the robot (e.g., 
the location of a plotting pen) and ignoring orientations, the planar 2R open 
chain with links of equal length three (Figure 2.12(a)) and the planar 3R open 
chain with links of equal length two (Figure 2.12(b)) have the same workspace 
despite having different C-spaces. 

Two mechanisms with the same C-space may also have different workspaces. 
For example, taking the end-effector to be the Cartesian tip of the robot and 
ignoring orientations, the 2R open chain of Figure 2.12(a) has a planar disk as 
its workspace, while the 2R open chain of Figure 2.12(c) has the surface of a 
sphere as its workspace. 

Attaching a coordinate frame to the tip of the tool of the 3R open chain 
“wrist” mechanism of Figure 2.12(d), we see that the frame can achieve any 
orientation by rotating the joints, but the Cartesian position of the tip is always 
fixed. This can be seen by noting that the three joint axes always intersect at 
the tip. For this mechanism, we would likely define the workspace to be the 
three-dof space of orientations of the frame, S 2 x5 1 , which is different from the 
C-space T 3 . The task space depends on the task; if the job is to point a laser 
pointer, then rotations about the axis of the laser beam are immaterial, and the 
task space would be S' 2 , the set of directions the laser can point. 

Example 2.7. The SCARA robot of Figure 2.13 is an RRRP open chain that is 
widely used for tabletop pick-and-place tasks. The encl-effector configuration is 
completely described by the four parameters (x,y,z,tp), where (x,y : z) denotes 
the Cartesian position of the end-effector center point, and </> denotes the ori¬ 
entation of the end-effector in the x-y plane. Its task space would typically be 
defined as K 3 x S 1 , and its workspace would typically be defined as the reachable 
points in (x, y , z) Cartesian space, since all orientations (f> G S 1 can be achieved 
at all reachable points. 
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Figure 2.13: SCARA robot. 



Figure 2.14: A spray-painting robot. 


Example 2.8. A standard 6R industrial manipulator can be adapted to spray¬ 
painting applications as shown in Figure 2.14. The paint spray nozzle attached 
to the tip can be regarded as the end-effector. What is important to the task 
is the Cartesian position of the spray nozzle, together with the direction in 
which the spray nozzle is pointing; rotations about the nozzle axis (which points 
in the direction in which paint is being sprayed) do not matter. The nozzle 
configuration can therefore be described by five coordinates: ( x,y,z ) for the 
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Cartesian position of the nozzle and spherical coordinates (6, <j>) to describe 
the direction in which the nozzle is pointing. The task space can be written 
as R 3 x S 2 . The workspace could be the reachable points in M 3 x S' 2 , or, to 
simplify visualization, the user could define the workspace to be the subset of 
R 3 corresponding to the reachable Cartesian positions of the nozzle. 

2.6 Summary 

• A robot is mechanically constructed from links that are connected by 
various types of joints. The links are usually modeled as rigid bodies. 
An end-effector such as a gripper may be attached to some link of the 
robot. Actuators deliver forces and torques to the joints, thereby causing 
motion of the robot. 

• The most widely used one-dof joints are the revolute joint, which allows 
for rotation about the joint axis, and the prismatic joint, which allows 
for translation in the direction of the joint axis. Some common two-dof 
joints include the cylindrical joint, which is constructed by serially con¬ 
necting a revolute and prismatic joint, and the universal joint, which is 
constructed by orthogonally connecting two revolute joints. The spheri¬ 
cal joint, also known as ball-in-socket joint, is a three-dof joint whose 
function is similar to the human shoulder joint. 

• The configuration of a rigid body is a specification of the location of all 
of its points. For a rigid body moving in the plane, three independent 
parameters are needed to specify the configuration. For a rigid body 
moving in three-dimensional space, six independent parameters are needed 
to specify the configuration. 

• The configuration of a robot is a specification of the configuration of all of 
its links. The robot’s configuration space is the set of all possible robot 
configurations. The dimension of the C-space is the number of degrees 
of freedom of a robot. 

• The number of degrees of freedom of a robot can be calculated using 

Grxibler’s formula, 

j 

dof = m(N - 1 - J) + 

2=1 

where m = 3 for planar mechanisms and m = 6 for spatial mechanisms, 
N is the number of links (including the ground link), J is the number of 
joints, and /* is the number of degrees of freedom of joint i. 

• A robot’s C-space can be parametrized explicitly or represented implicitly. 
For a robot with n degrees of freedom, an explicit parametrization uses 
n coordinates, the minimum necessary. An implicit representation 
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involves m coordinates with m > n, with the m coordinates subject to 
m — n constraint equations. With the implicit parametrization, a robot’s 
C-space can be viewed as a surface of dimension n embedded in a space 
of higher dimension m. 

• The C-space of an n-dof robot whose structure contains one or more closed 
loops can be implicitly represented using k loop-closure equations of 
the form g(6) = 0, where 9 £ R m and g : R m —> R k . Such constraint 
equations are called holonomic constraints. Assuming ( 9 (f) varies with 
time t, the holonomic constraints g(9(t)) = 0 can be differentiated with 
respect to f to yield 

§(W = o, 

where jfgifi) is a k x m matrix. 

• A robot’s motion can also be subject to velocity constraints of the form 

A{9)0 = 0, 

where A(9) is a k x m matrix that cannot be expressed as the differential 
of some function g(9), i.e., there does not exist any g{9),g : R m —f K fc , 
such that 

m = >>. 

Such constraints are said to be nonholonomic constraints, or noninte- 
grable constraints. These constraints reduce the dimension of feasible 
velocities of the system but do not reduce the dimension of the reach¬ 
able C-space. Nonholonomic constraints arise in robot systems subject to 
conservation of momentum or rolling without slipping. 

• A robot’s task space is a space in which the robot’s task can be naturally 
expressed. A robot’s workspace is a specification of the configurations 
the end-effector of the robot can reach. 


Notes and References 

In the kinematics literature, structures that consist of links connected by joints 
are also called mechanisms or linkages. The degrees of freedom of mechanisms 
is treated in most texts on mechanism analysis and design, e.g., [29]. A robot’s 
configuration space has the mathematical structure of a differentiable manifold. 
Some accessible introductions to differential manifolds and differential geometry 
are [88], [18]. Configuration spaces are further examined in a motion planning 
context in [59], [19]. 
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Figure 2.15: Robot used for human arm rehabilitation. 


2.7 Exercises 


1. Using the methods of Section 2.1, derive a formula, in terms of n , for the 
degrees of freedom of a rigid body in n-dimensional space. Indicate how many of 
those dof are translational and how many are rotational. Describe the topology 
of the C-space (e.g., for n = 2, the topology is R 2 x S' 1 ). 

2. Find the degrees of freedom of your arm, from your torso to your palm (just 
past the wrist, not including finger degrees of freedom). Do this in two ways: 

(a) add up the degrees of freedom at the shoulder, elbow, and wrist joints, and 

(b) fix your palm flat on a table with your elbow bent, and without moving your 
torso, investigate how many degrees of freedom with which you can still move 
your arm. Do your answers agree? How many constraints were placed on your 
arm when you placed your palm at a fixed configuration on the table? 

3. Assume each of your arms has n degrees of freedom. You are driving a car, 
your torso is stationary relative to the car (a tight seatbelt!), and both hands 
are firmly grasping the wheel, so that your hands do not move relative to the 
wheel. How many degrees of freedom does your arms-plus-steering wheel system 
have? Explain your answer. 

4. Figure 2.15 shows a robot used for human arm rehabilitation. Determine 
the degrees of freedom of the chain formed by the human arm and robot. 

5. The mobile manipulator of Figure 2.16 consists of a 6R arm and multifin¬ 
gered hand mounted on a mobile base with a single wheel. The wheel rotates 
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Figure 2.16: Mobile manipulator. 



Figure 2.17: Three cooperating SRS arms grasping a common object. 


without slip, and its axis of rotation always remains parallel to the ground. 

(a) Ignoring the multifingered hand, describe the configuration space of the mo¬ 
bile manipulator. 

(b) Now suppose the robot hand rigidly grasps the refrigerator door handle, and 
with its wheel completely stationary, opens the door using only its arm. With 
the door open, how many degrees of freedom does the mechanism formed by 
the arm and open door have? 

(c) A second identical mobile manipulator comes along, and after parking its 
mobile base, also rigidly grasps the refrigerator door handle. How many de¬ 
grees of freedom does the mechanism formed by the two arms and the open 
referigerator door have? 

6 . Three identical SRS open chain arms are grasping a common object as shown 
in Figure 2.17. 

(a) Find the degrees of freedom of this system. 

(b) Suppose there are now a total of n such arms grasping the object. What is 
the degrees of freedom of this system? 
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(c) Suppose the spherical wrist joint in each of the n arms is now replaced by a 
universal joint. What is the degrees of freedom of the overall system? 

7. Consider a spatial parallel mechanism consisting of a moving plate connected 
to the fixed plate by n identical open chain legs. In order for the moving plate 
to have six degrees of freedom, how many degrees of freedom should each leg 
have, as a function of n? For example, if n = 3, then the moving plate and fixed 
plate are connected by three open chains; how many degrees of freedom should 
each open chain have in order for the moving plate to move with six degrees of 
freedom? Solve for arbitrary n. 

8 . Use the planar version of Griibler’s formula to determine the degrees of free¬ 
dom of the mechanisms shown in Figure 2.18. Comment on whether your results 
agree with your intuition about the possible motions of these mechanisms. 

9. Use the spatial version of Griibler’s formula to determine the degrees of free¬ 
dom of the mechanisms shown in Figure 2.19. Comment on whether your results 
agree with your intuition about the possible motions of these mechanisms. 

10. In the parallel mechanism shown in Figure 2.20, six legs of identical length 
are connected to a fixed and moving platform via spherical joints. Determine 
the degrees of freedom of this mechanism using Griibler’s formula. Illustrate all 
possible motions of the upper platform. 

11. The 3 x UPU platform of Figure 2.21 consists of two platforms-the lower 
one stationary, the upper one mobile-connected by three UPU serial chains. 

(a) Using the spatial version of Griibler’s formula, Verify that it has three degrees 
of freedom. 

(b) Construct a physical model of the 3 x UPU platform to see if it indeed has 
three degrees of freedom. In particular, lock the three P joints in place; does the 
robot become a structure as predicted by Griibler’s formula, or does it move? 
Try reversing the order of the U joints, i.e., with the rotational axes connecting 
the leg to the platforms arranged parallel to the platforms, and also arranged 
orthogonal to the platforms. Does the order in which the U joints are connected 
matter? 

12. (a) The mechanism of Figure 2.22(a) consists of six identical squares ar¬ 
ranged in a single closed loop, connected serially by revolute joints. The bottom 
square is fixed to ground. Determine its degrees of freedom using an appropriate 
version of Griibler’s formula. 

(b) The mechanism of Figure 2.22(b) also consists of six identical squares con¬ 
nected by revolute joints, but arranged differently as shown. Determine its de¬ 
grees of freedom using an appropriate version of Griibler’s formula. Does your 
result agree with your intuition about the possible motions of this mechanism? 
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Figure 2.18: A collection of planar mechanisms. 


13. Figure 2.23 shows a spherical four-bar linkage, in which four links (one 
of the links is the ground link) are connected by four revolute joints to form a 
single-loop closed chain. The four revolute joints are arranged so that they lie 
on a sphere, and such that their joint axes intersect at a common point. 

(a) Use an appropriate version of Griibler’s formula to find the degrees of free¬ 
dom. Justify your choice of formula. 

(b) Describe the configuration space. 

(c) Assuming a reference frame is attached to the center link, describe its 
workspace. 

14. Figure 2.24 shows a parallel robot for surgical applications. As shown in 
Figure 2.24(a), leg A is an RRRP chain, while legs B and C are RRRUR chains. 
A surgical tool is attached to the end-effector as shown. 

(a) Use an appropriate version of Griibler’s formula to find the degrees of free¬ 
dom of the mechanism in Figure 2.24(a). 
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Figure 2.19: A collection of spatial parallel mechanisms. 



Figure 2.20: A 6 x SS platform. 


(b) Now suppose the surgical tool must always pass through point A in Fig¬ 
ure 2.24(a). How many degrees of freedom does the manipulator have? 

(c) Legs A, B, and C are now replaced by three identical RRRR legs as shown in 
Figure 2.24(b). Furthermore the axes of all R joints pass through point A. Use 
an appropriate version of Griibler’s formula to derive the degrees of freedom of 
this mechanism. 

15. Figure 2.25 shows a 3 x PUP platform, in which three identical PUP legs 
connect a fixed base to a moving platform. As shown in the figure, the P 
joints on both the fixed base and moving platform are arranged symmetrically. 
Recalling that the U joint consists of two revolute joints connected orthogonally, 
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Figure 2.21: The 3 x UPU platform. 




Figure 2.22: Two mechanisms. 


each R joint connected to the moving platform has its joint axis aligned in the 
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Figure 2.23: The spherical four-bar linkage. 


Leg C 




(a) 


(b) 


Figure 2.24: Surgical manipulator. 


same direction as the platform’s P joint. The R joint connected to the fixed base 
has its joint axis orthogonal to the base P joint. Use an appropriate version of 
Griibler’s formula to find the degrees of freedom. Does your answer agree with 
your intuition about this mechanism? If not, try to explain any discrepancies 
without resorting to a detailed kinematic analysis. 

16. The dual-arm robot of Figure 2.26 is rigidly grasping a box as shown. The 
box can only slide on the table; the bottom face of the box must always be in 
contact with the table. How many degrees of freedom does this system have? 
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Figure 2.25: The 3 x PUP platform. 



17. The dragonfly robot of Figure 2.27 has a body, four legs, and four wings 
as shown. Each leg is connected to its adjacent leg by a USP open chain. Use 
appropriate versions of Griibler’s formula to answer the following questions: 

(a) Suppose the body is fixed, and only the legs and wings can move. How many 
degrees of freedom does the robot have? 

(b) Now suppose the robot is flying in the air. How many degrees of freedom 
does the robot have? 

(c) Now suppose the robot is standing with all four feet in contact with the 
ground. Assume the ground is uneven, and that each foot-ground contact can 
be modeled as a point contact with no slip. How many degrees of freedom does 
the robot have? 
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Figure 2.27: Dragonfly robot. 




Figure 2.28: A caterpillar robot. 


18 . (a) A caterpillar robot is hanging by its tail as shown in Figure 2.28(a). 
The caterpillar robot consists of eight rigid links (one head, one tail, and six 
body links) connected serially by revolute-prismatic pairs as shown. Find the 
degrees of freedom of this robot. 

(b) The caterpillar robot is now crawling on a leaf as shown in Figure 2.28(b). 
Suppose all six body links must be in contact with the leaf at all times (each 
link-leaf contact can be modelled as a frictionless point contact). Find the de- 
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Figure 2.29: (a) A four-fingered hand with palm, (b) The hand grasping an 
ellipsoidal object, (c) A rounded fingertip that can roll on the object without 
sliding. 


grees of freedom of this robot during crawling. 

(c) Now suppose the caterpillar robot crawls on the leaf as shown in Fig¬ 
ure 2.28(c), in which only the first and last body links are in contact with 
the leaf. Find the degrees of freedom of this robot during crawling. 

19 . The four-fingered hand of Figure 2.29(a) consists of a palm and four URR 
fingers (the U joints connect the fingers to the palm). 

(a) Assume the fingertips are points, and that one of the fingertips is in contact 
with the table surface (sliding of the fingertip point contact is allowed). How 
many degrees of freedom does the hand have? WHat if two fingertips are in 
sliding point contact with the table? Three? All four? 

(b) Repeat part (a) but with each URR finger replaced by an SRR finger (each 
universal joint is replaced by a spherical joint). 

(c) The hand (with URR fingers) now grasps an ellipsoidal object as shown 
in Figure 2.29(b). Assume the palm is fixed in space, and that no slip occurs 
between the fingertips and object. How many degrees of freedom does the 
system have? 

(d) Now assume the fingertips are spheres as shown in Figure 2.29(c). Each 
of the fingertips can roll on the object, but cannot slip or break contact with 
the object. How many degrees of freedom does the system have? For a single 
fingertip in rolling contact with theobject, comment on the dimension of the 
space of feasible fingertip velocities relative to the object versus the number of 
parameters needed to represent the fingertip configuration relative to the object 
(its degrees of freedom). (Hint: You may want to experiment by rolling a ball 
around on a tabletop to get some intuition.) 

20 . Consider the slider-crank mechanism of Figure 2.4(b). A rotational motion 
at the revolute joint fixed to ground (the “crank”) causes a translational motion 
at the prismatic joint (the “slider”). Suppose the two links connected to the 
crank and slider are of equal length. Determine the configuration space of this 
mechanism, and draw its projected version on the space defined by the crank 
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Figure 2.30: Planar four-bar linkage. 


and slider joint variables. 

21. (a) Use an appropriate version of Grtibler’s formula to determine the degrees 
of freedom of a planar four-bar linkage floating in space. 

(b) Derive an implicit parametrization of the four-bar’s configuration space as 
follows. First, label the four links 1,2,3,4, and choose three points A,B,C on 
link 1, D,E,F on link 2, G,H,I on link 3, and J,K,L on link 4. The four- 
bar linkage is constructed such that the four following pairs of points are each 
connected by a revolute joint: C with D , F with G, I with J, and L with A. 
Write down explicit constraints on the coordinates for the eight points A,... H 
(assume a fixed reference frame has been chosen, and denote the coordinates 
for point A by pa = {xa, Va, za ), and similarly for the other points). Based on 
counting the number of variables and constraints, what is the degrees of freedom 
of the configuration space? If it differs from the result you obtained in (a), try 
to explain why. 

22 . In this exercise we examine in more detail the representation of the C-space 
for the planar four-bar linkage of Figure 2.30. Attach a fixed reference frame and 
label the joints and link lengths as shown in the figure. The (x, y) coordinates 
for joints A and B are given by 


A{9) = (a cos 9, asin#) T 

B(ip) = (g + bcosip, bsini/j) T . 


Using the fact that the link connecting A and B is of fixed length h, i.e., || A(9) — 
B (ip) || 2 = h 2 , we have the constraint 


b 2 + g 2 + 2gbcosijj + a 2 — 2(a cos 9(g + boos'll)) + a&sin0sint/>) = h 2 . 


Grouping the coefficients of cos if) and sin ip, the above equation can be expressed 
in the form 


a( 6 ) cos ip + / 3 ( 9 ) sin tj) = 7(0), 


(2.11) 
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Obstacle 


////////////// 


Figure 2.31: A circular disc robot moving in the plane. 


where 


a(0) = 2gb — 2abcos8 

/3(9) = — 2abs\n6 

7 (9) = h 2 — g 2 — b 2 — a 2 + 2ag cos 9. 


We now express ip as a function of 9 , by first dividing both sides of Equa¬ 
tion ( 2 . 11 ) by yja 2 + /3 2 to obtain 



7 


Va 2 + B 2 


Referring to Figure 2.30(b), the angle cp is given by <p = tan 1 (/3/a), so that 
Equation (22) becomes 



Therefore. 



(a) Note that a solution exists only if y 2 < a 2 + /3 2 . What are the physical 


implications if this constraint is not satisfied? 

(b) Note that for each value of input angle 9, there exists two possible values of 
the output angle ip. What do these two solutions look like? 

(c) Draw the configuration space of the mechanism in 6-ip space for the following 
link length values: a = b = g = h=l. 

(d) Repeat (c) for the following link length values: a = 1 , b = 2 , h = y/5, g = 2. 

(e) Repeat (c) for the following link length values: a = 1, b = 1, h = 1, g = y/3. 

23 . A circular disc robot moves in the plane as shown in Figure 2.31. An 
L-shaped obstacle is nearby. Using the (x, y) coordinates of the robot’s center 
as its configuration space coordinates, draw the free configuration space (i.e., 
the set of all feasible configurations) of the robot. 
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Figure 2.32: Two-link planar 2R open chain. 


24 . The tip coordinates for the two-link planar 2R robot of Figure 2.32 are 
given by 

x = 2 cos 9 1 + cos($i + 0 2 ) 
y = 2 sin 0i + sin(0i + 0 2 ). 

(a) What is the robot’s configuration space? 

(b) What is the robot’s task space (i.e., the set of all points reachable by the 
tip)? 

(c) Suppose infinitely long vertical barriers are placed at x = 1 and x = — 1. 
What is the free C-space of the robot (i.e., the portion of the C-space that does 
not result in any collisions with the vertical barriers)? 

25 . (a) Consider a planar 3R open chain with link lengths (starting from the 
fixed base joint) 5, 2, and 1, respectively. Considering only the Cartesian point 
of the tip, draw its workspace. 

(b) Now consider a planar 3R open chain with link lengths (starting from the 
fixed base joint) 1, 2, and 5, respectively. Considering only the Cartesian point of 
the tip, draw its workspace. Which of these two chains has a larger workspace? 

(c) A not-so-clever designer claims that he can make the workspace of any planar 
open chain larger simply by increasing the length of the last link. Explain the 
fallacy behind this claim. 

26 . (a) Describe the task space for a robot arm writing on a blackboard. 

(b) Describe the task space for a robot arm twirling a baton. 

27 . Give a mathematical description of the topologies of the C-spaces of the 
following systems. Use cross-products, as appropriate, of spaces such as a closed 
interval of a line [a, 6] and R fc , S m , and T n , where k , to, and n are chosen 
appropriately. 

(i) The chassis of a car-like mobile robot rolling on an infinite plane. 
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Side view 



Figure 2.33: A side view and a top view of a diff-drive robot. 


(ii) The car-like mobile robot, but including a representation of the wheel 
configurations. 

(iii) The car-like mobile robot driving around on a spherical asteroid. 

(iv) The car-like mobile robot on an infinite plane with an RRPR robot arm 
mounted on it. The prismatic joint has joint limits, but the revolute joints 
do not. 

(v) A free-flying spacecraft with a 6R arm mounted on it, no joint limits. 


28 . Describe an algorithm that drives the rolling coin of Figure 2.11 from any 
arbitrary initial configuration in its four-dimensional C-space to any arbitrary 
goal configuration, despite the two nonholonomic constraints. The control in¬ 
puts are the rolling speed 9 and the turning speed 0. 

29 . A differential-drive mobile robot has two wheels which do not steer but 
whose speeds can be controlled independently. The robot goes forward and 
backward by spinning the wheels in the same direction at the same speed, and 
it turns by spinning the wheels at different speeds. The configuration of the 
robot is given by five variables: the (x, y) location of the point halfway between 
the wheels, the heading direction 0 of the robot’s chassis relative to the x-axis of 
the world frame, and the rotation angles <f>i and 0 2 of the two wheels about the 
axis through the centers of the wheels (Figure 2.33). Assume that the radius of 
each wheel is r and the distance between the wheels is 2d. 

(i) Let q = (x, y, 0, 0i, 02) T be the configuration of the robot. If the two 
control inputs are the angular velocities of the wheels u>\ = 0i and 0 J 2 = 
02, write the vector differential equation q = gi(q)uj\+g 2 {q)u 2 - The vector 
fields g\(q) and 52 (<?) are called control vector fields, expressing how the 
system moves when the respective control is applied. 

(ii) Write the corresponding Pfaffian constraints A(q)q = 0 for this system. 
How many Pfaffian constraints are there? 
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(iii) Are the constraints holonomic or nonholonomic? Or how many are holo- 
nomic and how many nonholonomic? 


30 . Determine if the following differential constraints are holonomic or non¬ 
holonomic: 

(a) 

(1 + cos q\)qi + (1 + cos q 2 )q 2 + (cos q\ + cos q 2 + 4 )q 3 = 0. 

(b) 


-q 1 cosq 2 +<j 3 sin(gi + q 2 ) - q 4 cos(qi + q 2 ) = 0 

q 3 sin qi — 54 cos q\ = 0 . 
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Rigid-Body Motions 


In the previous chapter, we saw that a minimum of six numbers are needed to 
specify the position and orientation of a rigid body in three-dimensional physical 
space. In this chapter we develop a systematic way to describe a rigid body’s 
position and orientation that relies on attaching a reference frame to the body. 
The configuration of this frame with respect to a fixed reference frame is then 
represented as a 4 x 4 matrix. This is an example of an implicit representation 
of the C-space, as discussed in the previous chapter: the actual six-dimensional 
space of rigid-body configurations is obtained by applying ten constraints to the 
sixteen-dimensional space of 4 x 4 real matrices. 

Such a matrix not only represents the configuration of a frame, but it can 
also be used to (1) translate and rotate a vector or a frame, and (2) change the 
representation of a vector or a frame from coordinates in one frame (e.g., {a}) 
to coordinates in another frame (e.g., {b}). These operations can be performed 
by simple linear algebra, which is a major reason we choose to represent a 
configuration as a 4 x 4 matrix. 

The non-Euclidean (non-“flat”) nature of the C-space of positions and ori¬ 
entations leads us to use the matrix representation. A rigid body’s velocity, 
however, can be represented simply as a point in R 6 : three angular velocities 
and three linear velocities, which together we call a spatial velocity or twist. 
More generally, even though a robot’s C-space may not be Euclidean, the set of 
feasible velocities at any point in the C-space always forms a Euclidean space. 
As an example, consider a robot whose C-space is the sphere S 2 : although the 
C-space is not flat, the velocity at any configuration can be represented by two 
real numbers (an element of K 2 ), such as the rate of change of the latitude and 
the longitude. At any point on the sphere, the space of velocities can be thought 
of as the plane (a Euclidean space) tangent to that point on the sphere. 

Any rigid-body configuration can be achieved by starting from the fixed 
(home) reference frame and integrating a constant twist for a specified time. 
Such a motion resembles the motion of a screw, rotating about and translat¬ 
ing along the same fixed axis. The observation that all configurations can be 
achieved by a screw motion motivates a six-parameter representation of the 
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configuration called the exponential coordinates. The six parameters can be di¬ 
vided into parameters to describe the direction of the screw axis and a scalar 
to indicate how far the screw motion must be followed to achieve the desired 
configuration. 

This chapter concludes with a discussion of forces. Just as angular and linear 
velocities are packaged together into a single vector in M 6 , moments (torques) 
and forces are packaged together into a six-vector called a spatial force or wrench. 

To illustrate the concepts and to provide a synopsis of the chapter, we begin 
with a motivating planar example. Before doing so, we make some remarks 
about vector notation. 

A Word about Vectors and Reference Frames 

A free vector is a geometric quantity with a length and a direction. Think 
of it as an arrow in K n . It is called “free” because it is not necessarily rooted 
anywhere; only its length and direction matter. A linear velocity can be viewed 
as a free vector: the length of the arrow is the speed and the direction of the 
arrow is the direction of the velocity. A free vector is denoted by a regular text 
symbol, e.g., v. 

If a reference frame and length scale have been chosen for the underlying 
space in which v lies, then this free vector can be moved so the base of the arrow 
is at the origin, without changing the orientation. The free vector v can then 
be represented as a column vector in the coordinates of the reference frame. 
This vector is written in italics, v € R”, where v is at the “head” of the arrow 
in the frame’s coordinates. If a different reference frame and length scale are 
chosen, then the representation v will change, but the underlying free vector v 
is unchanged. 

In other words, we say that v is coordinate free ; it refers to a physical quantity 
in the underlying space, and it does not care how we represent it. On the other 
hand, v is a representation of v that depends on a choice of a coordinate frame. 

A point p in physical space can also be represented as a vector. Given 
a choice of reference frame and length scale for physical space, the point p 
can be represented as a vector from the reference frame origin to p; its vector 
representation is denoted in italics by p £ R n . Here, as before, a different 
choice of reference frame and length scale for physical space leads to a different 
representation p £ R™ for the same point p in physical space. See Figure 3.1. 

In the rest of this book, a choice of length scale will always be assumed, but 
we will be dealing with reference frames at different positions and orientations. 
A reference frame can be placed anywhere in space, and any reference frame 
leads to an equally valid representation of the underlying space and objects in 
it. However, we always assume that exactly one stationary fixed frame, or 
space frame, denoted {s}, has been defined. This might be attached to a 
corner of a room, for example. Similarly, we often assume that at least one 
frame has been attached to some moving rigid body, such as the body of a 
quadrotor flying in the room. The body frame, denoted {b}, is the stationary 
frame that is coincident with the body-attached frame at any instant. 
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P. 



Figure 3.1: The point p exists in physical space, and it does not care how we 
represent it. If we fix a reference frame {a}, with unit coordinate axes x a and y a , 
we can represent p as p a = (1, 2). If we fix a reference frame {b} at a different 
location, a different orientation, and a different length scale, we can represent p 
as p b = (4,-2). 


Important! All frames in this book are stationary, inertial frames. When 
we refer to a body frame {b}, we mean a motionless frame that is instanta¬ 
neously coincident with a frame that is fixed to a (possibly moving) body. 
This is important to keep in mind, since you may have had a dynamics 
course that used non-inertial moving frames. Do not confuse these with the 
stationary, inertial body frames of this book. 

For simplicity, we refer to a body frame as a frame attached to a moving 
rigid body. Despite this, at any instant, by “body frame” we mean the 
stationary frame that is coincident with the frame moving along with the 
body. 

It is worth repeating to yourself one more time: all frames are sta¬ 
tionary. 


While it is common to attach the origin of the {b} frame to some important 
point on the body, such as its center of mass, this is not required. The origin 
of the {b} frame might not even be on the physical body itself, as long as 
its location relative to the body, viewed from an observer on the body that is 
stationary relative to the body, is constant. 

3.1 Rigid-Body Motions in the Plane 

Consider the planar body of Figure 3.2, whose motion is confined to the plane. 
Suppose a length scale and a fixed reference frame have been chosen as shown. 
We call the fixed reference frame the fixed frame, or the space frame, denoted 
{s}, and label its unit axes x s and y s . (Throughout this book, the' notation 
indicates a unit vector.) Similarly, we attach a reference frame with unit axes 
x b and y b to the planar body. Because this frame moves with the body, it is 
called the body frame, and is denoted {b}. 
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Figure 3.2: The body frame {b} in fixed-frame coordinates {s} is represented 
by the vector p and the direction of the unit axes x b and y b expressed in {s}. 
In this example, p = (2,1) T and 8 = 60°, so x b = (cos0,sin0) T = (0.5, l/-\/2) T 
and y b = (— sin 0, cos 8) T = (—1/-\/2, 0.5) T . 


To describe the configuration of the planar body, only the position and ori¬ 
entation of the body frame with respect to the fixed frame needs to be specified. 
The body frame origin p can be expressed in terms of the coordinate axes of 
{s} as 

P = Px% + Pyfs- (3.1) 

You are probably more accustomed to writing this vector as simply p = ( p x ,Py)', 
this is fine when there is no possibility of ambiguity about reference frames, but 
writing p as in Equation (3.1) clearly indicates the reference frame with respect 
to which ( Px,P y ) is defined. 

The simplest way to describe the orientation of the body frame {b} relative 
to the fixed frame {s} is by specifying the angle 8 as shown in Figure 3.2. 
Another (admittedly less simple) way is to specify the directions of the unit 
axes x b and y b of {b} relative to {s}, in the form 

x b = cos 8 x s + sin 6 y s (3.2) 

y b = — sin0x s +cos0y s . (3.3) 

At first sight this seems a rather inefficient way to represent the body frame 
orientation. However, imagine the body were to move arbitrarily in three- 
dimensional space; a single angle 8 alone clearly would not suffice to describe 
the orientation of the displaced reference frame. We would in fact need three 
angles, but it is not yet clear how to define an appropriate set of three angles. 
On the other hand, expressing the directions of the coordinate axes of {b} in 
terms of coefficients of the coordinate axes of {s}, as we have done above for 
the planar case, is straightforward. 

Assuming we agree to express everything in terms of {s}, then just as the 
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Figure 3.3: The frame {b} in {s} is given by (P,p), and the frame {c} in {b} 
is given by ( Q , q). From these we can derive the frame {c} in {s}, described by 
(P, r). The numerical values of the vectors p , q , and r, and the coordinate axis 
directions of the three frames, are evident from the grid of unit squares. 


point p can be represented as a column vector p £ M 2 of the form 


P= „ , ( 3 - 4 ) 

Py 

the two vectors Xb and y b can also be written as column vectors and packaged 
into the following 2x2 matrix P, 


p = y b ] 


cos 9 — sin 9 
sin 9 cos 9 


(3.5) 


The matrix P is an example of a rotation matrix. Although P is constructed 
of four numbers, they are subject to three constraints (each column of P must 
be a unit vector, and the two columns must be orthogonal to each other), and 
the one remaining degree of freedom is parametrized by 9. Together, the pair 
(P,p) provides a description of the orientation and position of {b} relative to 
{s}. 

Now refer to the three frames in Figure 3.3. Repeating the approach above, 
and expressing {c} in {s} as the pair (P, r), we can write 



R = 


cos (f> — sin <t> 
sin <fi cos cf> 


(3.6) 


We could also describe the frame {c} relative to {b}. Letting q denote the 
vector from the origin of {b} to the origin of {c} expressed in {b} coordinates, 
and letting Q denote the orientation of {c} relative to {b}, we can write {c} 
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relative to {b} as the pair (Q,q), where 


q = 


q x 

q y 


Q 


cos ip — sin ip 
sin ip cos ip 


(3.7) 


If we know ( Q,q ) (the configuration of {c} relative to {b}) and (P,p) (the 
configuration of {b} relative to {s}), we can compute the configuration of {c} 
relative to {s} as follows: 


R = PQ (convert Q to the {s} frame) (3.8) 

r = Pq + p (convert q to the {s} frame and vector sum with p). (3.9) 

Thus (P,p) not only represents a configuration of {b} in {s}; it can also be used 
to convert the representation of a point or frame from {b} coordinates to {s} 
coordinates. 

Now consider a rigid body with two frames attached to it, {d} and {c}. The 
frame {d} is initially coincident with {s}, and {c} is initially described by (R, r) 
in {s} (Figure 3.4(a)). Then the body is moved so that {d} moves to {d 7 }, 
coincident with a frame {b} described by (P,p) in {s}. Where does {c} end up 
after this motion? Denoting ( R',r') as the configuration of the new frame {c 7 }, 
you can verify that 

R' = PR (3.10) 

r' = Pr +p, (3-11) 


very similar to Equations (3.8) and (3.9). The difference is that (P,p) is ex¬ 
pressed in the same frame as (R, r ), so the equations are not viewed as a change 
of coordinates, but instead as a rigid-body displacement (also known as a 
rigid-body motion) that ® rotates {c} according to P and @ translates it 
by p in {s}. See Figure 3.4(a). 

Thus we see that a rotation matrix-vector pair such as (P,p) can be used to 
do three things: 

(i) Represent a configuration of a rigid body in {s} (Figure 3.2). 

(ii) Change the reference frame in which a vector or frame is represented 
(Figure 3.3). 

(iii) Displace a vector or a frame (Figure 3.4(a)). 

Referring to Figure 3.4(b), note that the rigid-body motion illustrated in 
Figure 3.4(a), expressed as a rotation followed by a translation, can be obtained 
by simply rotating the body about a fixed point s by an angle f3. This is a planar 
example of a screw motion . 1 The displacement can therefore be parametrized 
by the three screw coordinates (/?, s x , s y ), where (s x , s y ) denote the coordinates 
for the point s (i.e., the screw axis) in the fixed-frame {s}. 

1 If the displacement is a pure translation without rotation, then s lies at infinity. 
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Figure 3.4: (a) The frame {d}, fixed to an elliptical rigid body and initially coin¬ 
cident with {s}, is displaced to {d'} (coincident with the stationary frame {b}), 
by first rotating according to P then translating according to p, where (P, p ) is 
the representation of {b} in {s}. The same transformation takes the frame {c}, 
also attached to the rigid body, to {c'}. The transformation marked ® rigidly 
rotates {c} about the origin of {s}, and then transformation @ translates the 
frame by p expressed in {s}. (b) Instead of viewing this displacement as a rota¬ 
tion followed by a translation, both rotation and translation can be performed 
simultaneously. The displacement can be viewed as a rotation of /? = 90° about 
a fixed point s. 


Another way to represent the screw motion is to consider it the displacement 
obtained by following a simultaneous angular and linear velocity for a given 
amount of time. Inspecting Figure 3.4(b), we see that rotating about s with 
a unit angular velocity (w = 1 rad/s) means that a point at the origin of 
the {s} frame moves at two units per second in the +x direction of the {s} 
frame (v = (v x ,v y ) = (2,0)). Packaging these together in a 3-vector as V = 
(uj, v x ,v v ) = (1,2,0), we call this the planar twist (velocity) corresponding 
to a unit angular velocity rotation about s. Following this planar twist for 
time (or angle) t = 7r/2 yields the final displacement. We can now express the 
screw motion (/3,s x ,s y ) in the alternate form Vt = (7r/2,7r,0). This form has 
some advantages, and we call these coordinates the exponential coordinate 
representation of the planar rigid-body displacement. 
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Figure 3.5: Mathematical description of position and orientation. 


Remainder of the chapter. In the remainder of this chapter we generalize 
the concepts above to three-dimensional rigid-body motions. For this purpose 
consider a rigid body occupying three-dimensional physical space as shown in 
Figure 3.5. Assume that a length scale for physical space has been chosen, and 
that both the fixed frame {s} and body frame {b} have been chosen as shown. 
Throughout this book all reference frames are right-handed, i.e., the unit axes 
{x, y, z} always satisfy x x y = z. Denote the unit axes of the fixed frame by 
{x s ,y s ,z s } and the unit axes of the body frame by {xb,y b ,Zb}. Let p denote 
the vector from the fixed frame origin to the body frame origin. In terms of the 
fixed frame coordinates, p can be expressed as 

p = Pix s + P2y s + p 3 z s . (3.12) 

The axes of the body frame can also be expressed as 


Xb = rnx s + r 2 iy s + r 3 iz s 
y b = r i 2 X s + r 22 y s + r 32 z s 
2b = r-i 3 x s + r 23 y s + r 33 z s . 

Defining p € R 3 and Ji £ R 3x3 as 


Pi 


rn 

rn 

n 3 

Pi 

, R = [xb Yb z b ] = 

V21 

T22 

r23 

. P3 . 


_ r 3 i 

r 3 2 

r 33 _ 


(3.13) 

(3.14) 

(3.15) 


(3.16) 


the twelve parameters given by ( R , p) then provide a description of the position 
and orientation of the rigid body relative to the fixed frame. 

Since the orientation of a rigid body has three degrees of freedom, only 
three of the nine entries in R can be chosen independently. One three-parameter 
representation of rotations are the exponential coordinates, which define an axis 
of rotation and the distance rotated about that axis. We leave other popular 
representations of orientations (three-parameter Euler angles and roll-pitch- 
yaw angles, the Cayley-Rodrigues parameters, and the unit quaternions 
that use four variables subject to one constraint) to Appendix B. 









3.2. Rotations and Angular Velocities 


59 


We then examine six-parameter exponential coordinates for the configuration 
of a rigid body that arise from integrating a six-dimensional twist consisting 
of the body’s angular and linear velocity. This representation follows from the 
Chasles-Mozzi theorem that states that every rigid-body displacement can be 
described as a finite rotation and translation about a fixed screw axis. 

We conclude with a discussion of forces and moments. Rather than treat 
these as separate three-dimensional quantities, we merge the moment and force 
vectors into a six-dimensional wrench. The twist and wrench, and rules for 
manipulating them, form the basis for the kinematic and dynamic analyses in 
the subsequent chapters. 


3.2 Rotations and Angular Velocities 

3.2.1 Rotation Matrices 


We argued earlier that of the nine entries in the rotation matrix R, only three can 
be chosen independently. We begin by expressing a set of six explicit constraints 
on the entries of R. Recall that the three columns of R correspond to the 
body frame’s unit axes {x b ,y b ,z b }. The following conditions must therefore be 
satisfied: 


(i) Unit norm condition: x b , y b , and z b are all of unit norm, or 


r 

r 

r 


2 

11 
2 

12 
2 

13 


+ r 22 
+ r 23 


+ r 32 
+ r 33 


1 

1 

1 . 


(3.17) 


(ii) Orthogonality condition: x b • y b = x b • z b = y b • z b = 0 (here • denotes the 
inner product), or 


rur 12 + r 2 ir 2 2 + r 31 r 32 = 0 

i2i* 13 + r 22 r 23 + r 32 r 33 = 0 

riiri 3 + r 2 ir 23 + r 31 r 33 = 0. 


(3.18) 


These six constraints can be expressed more compactly as a single set of con¬ 
straints on the matrix R, 

R t R = I, (3.19) 

where R T denotes the transpose of R and I denotes the identity matrix. 

There is still the matter of accounting for the fact that the frame is right- 
handed (i.e., x b x y b = z b , where x denotes the cross-product) rather than 
left-handed (i.e., x b x y b = — z b ); our six equality constraints above do not dis¬ 
tinguish between right- and left-handed frames. We recall the following formula 
for evaluating the determinant of a 3 x 3 matrix M : denoting the three columns 
of M by a, b , and c, respectively, its determinant is given by 

detM = a T (b x c) = c T (a x b) = b T (c x a). 


(3.20) 
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Substituting the columns for R into this formula then leads to the constraint 

det R = 1. (3.21) 

Note that if the frame had been left-handed, we would have detR = — 1. In 
summary, the six equality constraints represented by (3.19) imply that detR = 
±1; imposing the additional constraint detR = 1 means that only right-handed 
frames are allowed. The constraint det R = 1 does not change the number of 
independent continuous variables needed to parametrize R. 

The set of 3 x 3 rotation matrices forms the Special Orthogonal Group 
50(3), which we now formally define: 

Definition 3.1. The Special Orthogonal Group SO( 3), also known as the 
group of rotation matrices, is the set of all 3 x 3 real matrices R that satisfy (i) 
R T R = / and (ii) det R = 1. 

The set of 2 x 2 rotation matrices is a subgroup of 50(3), and is denoted 
50(2). 

Definition 3.2. The Special Orthogonal Group 50(2) is the set of all 2x2 

real matrices R that satisfy (i) R T R = I and (ii) detR = 1. 

From the definition it follows that every R £ 50(2) can be written 


I'll 

fl2 


cos 9 

— sin0 

_ r 2 1 

t-22 


sin0 

cos 8 


where 6 £ [0, 2-7 t). Elements of 50(2) represent planar orientations and elements 
of 50(3) represent spatial orientations. 

3.2.1.1 Properties of Rotation Matrices 

The sets of rotation matrices 50(2) and 50(3) are called “groups” because 
they satisfy the properties required of a mathematical group. 2 Specifically, a 
group consists of a set of elements and an operation on two elements (matrix 
multiplication for SO{n)) such that, for all A, B in the group, the following 
properties are satisfied: 

• closure: AB is also in the group. 

• associativity: ( AB)C = A(BC). 

• identity element existence: There exists an I in the group (the identity 
matrix for 50(n)) such that AI = IA = A. 

• inverse element existence: There exists an A~ x in the group such that 
AA- 1 = A- 1 A = I. 

2 More specifically, the SO(n) groups are also called Lie groups , where “Lie” is pronounced 
“lee,” because the elements of the group form a differentiable manifold. 
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Proofs of these properties are given below, using the fact that the identity 
matrix / is a trivial example of a rotation matrix. 

Proposition 3.1. The inverse of a rotation matrix R € SO( 3) is also a rotation 
matrix, and it is equal to the transpose of R, i.e., R _1 = R T . 

Proof. The condition R T R = I implies that l? -1 = R T and RR T = I. Since 
detl? T = det I? = 1, R T is also a rotation matrix. □ 

Proposition 3.2. The product of two rotation matrices is a rotation matrix. 

Proof. Given 6 50(3), their product R 1 R 2 satisfies (i?ii? 2 ) T (R 1 R 2 ) = 

pQf R\ R 1 R 2 = R 2 5 2 = I. Further, det R 1 R 2 = det R± • det i? 2 = 1. Thus R 1 R 2 
satisfies the conditions for a rotation matrix. □ 

Proposition 3.3. Multiplication of rotation matrices is associative, (l?il? 2 )l?3 = 
i?i(i?. 2 l?3), but generally not commutative, R 1 R 2 ^ i? 2 l?i. For the special case 
of rotation matrices in 50(2), rotations commute. 

Proof. Associativity and non-commutativity follows from properties of matrix 
multiplication in linear algebra. Commutativity for planar rotations follows 
from a direct calculation. Q 

Another important property is that the action of a rotation matrix on a 
vector (e.g., rotating the vector) does not change the length of the vector. 

Proposition 3.4. For any vector x £ R 3 and R € 50(3), the vector y = Rx 
is of the same length as x. 

Proof. This follows from ||y|| 2 = y T y = (Rx) T Rx = x T R T Rx = x T x = ||x|| 2 . 

□ 


3.2.1.2 Uses of Rotation Matrices 

Analogous to Section 3.1, there are three major uses for a rotation matrix R: 

(i) Represent an orientation. 

(ii) Change the reference frame in which a vector or a frame is represented. 

(iii) Rotate a vector or a frame. 

In the first use, R is thought of as representing a frame; in the second and third 
uses, R is thought of as an operator that acts on a vector or frame (changing 
its reference frame or rotating it). 

To illustrate these uses, refer to Figure 3.6, which shows three different 
coordinate frames—{a}, {b}, and {c}—representing the same space. 3 These 
frames have the same origin, since we are only representing orientations, but 

3 In the rest of the book, all coordinate frames will use the same length scale; only their 
position and orientation may be different. 



62 


Rigid-Body Motions 



«' • • 

P P P 


Figure 3.6: The same space and the same point p represented in three different 
frames with different orientations. 


to make the axes clear, the figure shows the same space drawn three times. A 
point p in the space is also shown. Not shown is a fixed space frame {s}, which 
is aligned with {a}. The orientations of the three frames relative to {s} can be 
written 
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and the location of the point p in these frames can be written 



' 1 ■ 


1 ■ 


0 ' 

Pa = 

1 

, Pb = 

-1 

) Pc = 

-1 


0 


0 


-1 


Note that {b} is obtained by rotating {a} about z a by 90°, and {c} is obtained 
by rotating {b} about y b by —90°. (The direction of positive rotation about an 
axis, 9 > 0, is determined by the direction the fingers of your right hand curl 
about the axis when you point the thumb of your right hand along the axis.) 

Representing an orientation. When we write R c , we are implicitly referring 
to the orientation of frame {c} relative to the fixed frame {s}. We can be more 
explicit about this by writing it as R sc : we are representing the frame of the 
second subscript, {c}, relative to the frame of the first subscript, {s}. This 
notation allows us to express a frame relative to a frame that is not {s}; for 
example, Rb c is the orientation of {c} relative to {b}. 

If there is no possibility of confusion regarding the frames involved, we may 
simply write R. 

Inspecting Figure 3.6, we see that 


' 0 

-1 

0 ■ 
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0 

1 ■ 
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-1 

i Rea — 

-1 
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A simple calculation shows that R ac Rca = I, he-, Rac = R C a i or, equivalently, 
from Proposition 3.1, R ac = Rea ■ I n fact, f° r an y two frames {d} and {e}, 

Rde = R ed = Red- 
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You can verify this fact using any two frames in Figure 3.6. 

Changing the reference frame. The rotation matrix R a i, represents the 
orientation of {b} in {a}, and Rb c represents the orientation of {c} in {b}. 
A straightforward calculation shows that the orientation of {c} in {a} can be 
computed as 

Rac = RabRbc■ (3.22) 

In the previous equation, Rb c can be viewed as a representation of an orientation, 
while R a b can be viewed as a mathematical operator that changes the reference 
frame from {b} to {a}, i.e., 

Rac = RabRbc = change_reference_frameTrom_{b}_to_{a} (Rbc)- 

A subscript cancellation rule helps to remember this property. When multi¬ 
plying two rotation matrices, if the second subscript of the first matrix matches 
the first subscript of the second matrix, the two subscripts cancel and a change 
of reference frame is achieved: 

RabRbc — ^afi^jtc — Rac- 

A rotation matrix is just a collection of three unit vectors, so the reference 
frame of a vector can also be changed by a rotation matrix using a modified 
version of the subscript cancellation rule: 

RabPb = R'jPf = Pa- 

You can verify these properties using the frames and points in Figure 3.6. 

Rotating a vector or a frame. The last use of a rotation matrix is to rotate 
a vector or a frame. Figure 3.7 shows a frame {c} initially aligned with {s} with 
axes {x, y, z}. If we rotate the {c} frame about a unit axis Cj by an amount 0, the 
new {c 7 } frame has coordinate axes {x , ,y , ,z / }. The rotation matrix R = R sc > 
represents the orientation of {c / } relative to {s}, but instead we can think of it 
as representing the rotation operation itself. Emphasizing our view of I? as a 
rotation operator, instead of as an orientation, we can write 

R = Rot(d), 9), 

the operation that rotates the orientation represented by the identity matrix 
to the orientation represented by R. As we will see in Section 3.2.3.3, for 

U> = (wi,W 2 ,W 3 ), 

Rot(w,0) = 

Cg + LOi(l ~ Cg) WiW 2 (l — Cg) — W 3 Sg Wid) 3 (l — Cg) + W 2 Sg 

WiW 2 (l - Cg) + LU 3 Sg Cg + w|(l - Cg) W 2 W 3 (1 - Cg) - WiSg , 

_ 0>iW 3 (l - Ce) - LU 2 Sg d) 2 W 3 (l - Cg) + W 1 S 9 Cg + w|(l - Cg) 
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Figure 3.7: A coordinate frame with axes {x, y, z} is rotated an amount 9 about 
a unit axis Cj (which is aligned with —y in this figure). The orientation of the 
final frame, with axes {x , ,y , ,z / }, is written as R relative to the original frame. 

where sg = sin0 and c g = cos 6. Any R G SO( 3) can be obtained by rotating 
from the identity matrix by some 9 about some Cj. 

Now say R s g represents some {b} relative to {s}, and we want to rotate 
{b} by 9 about a unit axis Cj, i.e., R = Rot(w,0). To be clear about what we 
mean, we have to specify whether the axis of rotation Cj is expressed in {s} 
coordinates or {b} coordinates. Depending on our choice, the same numerical 
Cj (and therefore the same numerical R) corresponds to different rotation axes 
in the underlying space, unless the {s} and {b} frames are aligned. Letting 
{b 7 } be the new frame after rotating by 9 about Cj s = Cj (the rotation axis Cj 
is considered to be in the fixed {s} frame), and {b"} be the new frame after 
rotating by 9 about Cjb = Cj (the rotation axis Cj is considered to be in the body 
{b} frame), representations of these new frames can be calculated as 

R s b> = rotate_by_i?_in_{s}.frame ( R s g ) = RR s b (3.23) 

Rsb" = rotate_by_i?_in_{b}-frame ( R s b ) = R s bR • (3.24) 

In other words, premultiplying by R = Rot(w,0) yields a rotation about Cj 
considered to be in the fixed frame, and postmultiplying by R yields a rotation 
about Cj considered to be in the body frame. 

Rotating by R in the {s} frame and the {b} frame is illustrated in Figure 3.8. 

To rotate a vector v , note that there is only one frame involved, the frame 
that v is represented in, and therefore Cj must be interpreted in this frame. The 
rotated vector v' , in that same frame, is 


v' = Rv. 


3.2.1.3 Other Representations of Orientations 

Other popular representations of orientations include Euler angles, roll-pitch- 
yaw angles, the Cayley-Rodrigues parameters, and unit quaternions. These 
representations, and their relation to rotation matrices, are discussed in Ap¬ 
pendix B. 
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Figure 3.8: (Top) A rotation operator R defined as R = Rot(f, 90°), the orienta¬ 
tion of the right frame in the left frame. (Bottom) On the left are shown a fixed 
frame {s} and a body frame {b}, expressed as R s b- The quantity RR s b rotates 
{b} to {b'} by rotating by 90° about the fixed frame axis z s . The quantity R s bR 
rotates {b} to {b"} by rotating by 90° about the body frame axis z^. 

3.2.2 Angular Velocity 

Referring to Figure 3.9(a), suppose a body frame with unit axes {x,y, z} is 
attached to a rotating body. Let us determine the time derivatives of these unit 
axes. Beginning with x, first note that x is of unit length; only the direction 
of x can vary with time (the same goes for y and z). If we examine the body 
frame at times t and t +At, the change in frame orientation can be described as 
a rotation of angle A 8 about some unit axis w passing through the origin. The 
axis w is coordinate-free; it is not yet represented in any particular reference 
frame. 

In the limit as At approaches zero, the ratio becomes the rate of rotation 
6, and w can similarly be regarded as the instantaneous axis of rotation. In fact, 
w and 9 can be put together to define the angular velocity w as follows: 


w = w 6. 


(3.25) 
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Figure 3.9: (Left) The instantaneous angular velocity vector. (Right) Calculat¬ 
ing x. 


Referring to Figure 3.9(b), it should be evident that 


X 

= W X X 

(3.26) 

y 

= w x y 

(3.27) 

z 

= w x z. 

(3.28) 


To express these equations in coordinates, we have to choose a reference 
frame in which to represent w. We can choose any reference frame, but two 
natural choices are the fixed frame {s} and the body frame {b}. Let us start 
with fixed frame {s} coordinates. Let R(t) be the rotation matrix describing 
the orientation of the body frame with respect to the fixed frame at time f; R(t) 
is its time rate of change. The first column of f?(f), denoted r 1 (f), describes x 
in fixed frame coordinates; similarly, r 2 (t) and ra(t) respectively describe y and 
z in fixed frame coordinates. At a specific time t, let oj s £ 1R 3 be the angular 
velocity w expressed in fixed frame coordinates. Then Equations (3.26)-(3.28) 
can be expressed in fixed frame coordinates as 


A = to s x r it * = 1,2, 3. 


These three equations can be rearranged into the following single 3x3 matrix 
equation: 

R = [ui s x ri | oj s x r-i \ ui s x 7 - 3 ] = u> s x R. (3.29) 

To eliminate the cross product in Equation (3.29), we introduce some new 
notation and rewrite x R as [w s ]i?, where [ta s ] is a 3 x 3 skew-symmetric 
matrix representation of uj s £t 3 : 

Definition 3.3. Given a vector x = (x\,X 2 ,X 3 ) T £ R 3 , define 


x = 


0 — X 3 x 2 

0 —xi 
x\ 0 


X3 

-x 2 


(3.30) 
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The matrix [x] is a 3 x 3 skew-symmetric matrix representation of x; that is, 

.1T 


X = - X 


The set of all 3x3 real skew-symmetric matrices is called so(3). 4 

A useful property involving rotations and skew-symmetric matrices is the 
following: 

Proposition 3.5. Given any u £ R 3 and R £ SO( 3), the following always 
holds: 

R[uj\R t = [Rcu]. (3.31) 


rf (u) x 

ri) 

rf(u x 

1"2) 

r[(ui x r 3 ) 


rj(w x 

n) 

rj(w x 

ri) 

(u x r 3 ) 


rl (u x 

n) 

rj(w x 

r2) 

ri (w x r 3 ) _ 


0 

~ r 3 

T 1 

uj 


(3.32) 

T 

H CJ 

0 

-rj 

id 


T 

-r$uj 

rfu: 0 




Proof. Letting rf be the *th row of R 

R[lo]R t = 

= [Ru 

where the second line makes use of the determinant formula for 3x3 matrices, 
i.e., if M is a 3 x 3 matrix with columns {a, b, c}, then det M = a T {b x c) = 
c T (a x b) = b T (c x a). □ 

With the skew-symmetric notation, we can rewrite Equation (3.29) as 

[w s ]i? = R. (3.33) 

We can post-multiply both sides of Equation (3.33) by R~ 1 to get 


fwJ = RR 


(3.34) 


Now let ojb be w expressed in body frame coordinates. To see how to obtain 
u>b from u s and vice versa, we explicitly write R as R s b- Then co s and u>b are 
two different vector representations of the same angular velocity w, and by our 
subscript cancellation rule, oj s = R s b<^b- Therefore 

ojb = R~bU s = 7? _1 w s = R t u) s . (3.35) 

Let us now express this relation in skew-symmetric matrix form: 

M = [R T u; s ] 

= R t [uj s ]R (by Proposition 3.5) 

= R t (RR t )R 
= R T R = R-'R. 


4 The set of skew-symmetric matrices so(3) is called the Lie algebra of the Lie group SO (3). 
It consists of all possible R when R = I. 
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In summary, we have the following two equations that relate R and R to the 
angular velocity w: 

Proposition 3.6. Let R(t) denote the orientation of the rotating frame as seen 
from the fixed frame. Denote by w the angular velocity of the rotating frame. 
Then 


RR- 1 = [w s ] (3.37) 

R^R = [w b ], (3.38) 

where uj s £ R 3 is the fixed frame vector representation of w and [w s ] £ so(3) is its 
3x3 matrix representation, and w b £ R 3 is the body frame vector representation 
of w and [w b ] £ so(3) is its 3 x 3 matrix representation. 

It is important to note that w b is not the angular velocity relative to a moving 
frame. Instead, w b is the angular velocity relative to the stationary frame {b} 
that is instantaneously coincident with a frame attached to the moving body. 

It is also important to note that the fixed-frame angular velocity oj s does not 
depend on the choice of the body frame. Similarly, the body-frame angular veloc¬ 
ity u>b does not depend on the choice of the fixed frame. While Equations (3.37) 
and (3.38) may appear to depend on both frames (since R and R individually 
depend on both {s} and {b}), the product is independent of {b} and the 

product R—^R is independent of {s}. 

Finally, an angular velocity expressed in an arbitrary frame {d} can be 
represented in another frame {c} if we know the rotation that takes {c} to {d}, 
using our now-familiar subscript cancellation rule: 

^c “ Rcd^d- 

3.2.3 Exponential Coordinate Representation of Rotation 

We now introduce a three-parameter representation for rotations, the expo¬ 
nential coordinates for rotation. The exponential coordinates parametrize 
a rotation matrix in terms of a rotation axis (represented by a unit vector Cj ), 
together with an angle of rotation 9 about that axis; the vector CjQ £ R 3 then 
serves as the three-parameter exponential coordinate representation of the ro¬ 
tation. This representation is also called the axis-angle representation of a 
rotation, but we prefer to use the term “exponential coordinates” to emphasize 
the connection to the upcoming exponential coordinates for rigid-body trans¬ 
formations. 

The exponential coordinates for a rotation can be interpreted equivalently 
as: 


• rotating about the axis Cj by 6 ; 

• integrating the angular velocity Cj6 for one second; or 

• integrating the angular velocity Cj for 6 seconds. 
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The latter two views suggest that we consider exponential coordinates in the 
setting of linear differential equations. Below we briefly review some key results 
from linear differential equations. 


3.2.3.1 Essential Results from Linear Differential Equations 

Let us begin with the simple scalar linear differential equation 

x(t) = ax(t), (3.39) 

where x{t) £ M, a £ R is constant, and the initial condition x(0) = xq is assumed 
given. Equation (3.39) has solution 

x(t) = e at XQ. 


It is also useful to remember the series expansion of the exponential function: 


at (of) 2 (at) 3 

e at = 1 + at + + ... 


2! 3! 

Now consider the vector linear differential equation 


x(t) = Ax{t) 


(3.40) 


where x(t) £ A £ R nxn is constant, and the initial condition x(0) = Xq is 
given. From the earlier scalar result, one can conjecture a solution of the form 

x{t) = e At xo (3-41) 

where the matrix exponential e At now needs to be defined in a meaningful way. 
Again mimicking the scalar case, we define the matrix exponential to be 


e 


At 


I T At + 


(At ) 2 

2 ! 


+ 


(At) 3 

3! 


(3.42) 


The first question to be addressed is under what conditions this series converges, 
so that the matrix exponential is well-defined. It can be shown that if A is 
constant and finite, this series is always guaranteed to converge to a finite limit; 
the proof can be found in most texts on ordinary linear differential equations 
and is not covered here. 

The second question is whether Equation (3.41), using Equation (3.42), is 
indeed a solution to Equation (3.40). Taking the time derivative of x(t) = e At Xo, 


x(t) 



d 

dt 


^1 + At + 


AH 2 

2 ! 


+ A 2 t + 


A 3 t 2 

2 ! 


Ae At x o 
Ax(t), 


A 3 t 3 

3! 



x 0 


(3.43) 
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which proves that x(t) = e At Xo is indeed a solution. That this is a unique 
solution follows from the basic existence and uniqueness result for linear ordinary 
differential equations, which we invoke here without proof. 

While AB BA for arbitrary square matrices A and B, it is always true 
that 

Ae M = e M A (3.44) 

for any square A and scalar t. You can verify this directly using the series 
expansion for the matrix exponential. Therefore, in line four of Equation (3.43), 
A could also have been factored to the right, i.e., 

x(t) = e M AxQ. 

While the matrix exponential e At is defined as an infinite series, closed- 
form expressions are often available. For example, if A can be expressed as 
A = PDP -1 for some D £ R raxn and invertible P £ R nX ", then 


At 


= I 


A (At) 2 


= I + (PDP- 1 )t + (PDP- 1 )(PDP- 1 )- + . 
= P(I + Dt+ ( ^^ + ...)P~ 1 


2 ! 


(3.45) 


= Pe P 


Dt TJ— 1 


If moreover D is diagonal, i.e., D = diag{di,d 2 ) • • • ,d„}, then its matrix expo¬ 
nential is particularly simple to evaluate: 


e d it o 
0 e d2t 


0 

0 


0 0 


e 


d n t 


(3.46) 


We summarize the results above in the following proposition. 

Proposition 3.7. The linear differential equation x(t) = Ax(t) with initial 
condition x(0) = xq, where A £ R” xn is constant and x(t) £ K™, has solution 

x(t) = e At xo (3.47) 


where ^ 

e At = I + tA+t-A 2 yt-A 3 + .... (3.48) 

The matrix exponential e At further satisifies the following properties: 

(i) A e At = Ae At = e At A. 

(ii) If A — PDP _1 for some D £ K" xn and invertible P £ R” xra , then 
e At = Pe Dt P - 1 . 
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(iii) If AB = BA, then e A e B = e A+B . 

(iv) (e" 4 ) -1 = e~ A . 

The third property can be established by expanding the exponentials and 
comparing terms. The fourth property follows by setting B = —A in the third 
property. 

A final linear algebraic result useful in finding closed-form expressions for 
e At is the Cayley-Hamilton Theorem, which we state here without proof: 

Proposition 3.8. Let A £ R nxn be constant, with characteristic polynomial 

p(s) = det (si — A) = s n + Cn-is"” 1 + ... + cis + cq, 

and define p(A) as 

p(A) =A n + c n - 1 A n ~ 1 + ... + ciA + col. 


Then p{A) = 0. 

3.2.3.2 Exponential Coordinates of Rotations 

The exponential coordinates of a rotation can be viewed equivalently as (1) a 
unit axis of rotation Cj (Cj £ R 3 , ||w|| = 1) together with a rotation angle about 
the axis 9 £ R, or (2) as the three-vector obtained by multiplying the two 
together, aid £ R 3 . When we represent the motion of a robot joint in the next 
chapter, the first view has the advantage of separating the description of the 
joint axis from the motion 9 about the axis. 

Referring to Figure 3.10, suppose a three-dimensional vector p(0) is rotated 
by 9 about Cj to p(9); here we assume all quantities are expressed in fixed frame 
coordinates. This rotation can be achieved by imagining that p(0) rotates at a 
constant rate of 1 rad/s (since C is unit) from time t = 0 to t = 9. Let p(t) 
denote this path. The velocity of p(t), denoted p, is then given by 

p = Cj x p. (3.49) 

To see why this is true, let </> be the angle between p(t) and Cj. Observe that p 
traces a circle of radius ||p|| sin</> about the cD-axis. Then p = Cj x p is tangent 
to the path with magnitude ||p|| sin 0, which is exactly Equation (3.49). 

The differential equation (3.49) can be expressed as 


V = [w]p (3.50) 

with initial condition p(0). This is a linear differential equation of the form 
x = Ax that we studied earlier; its solution is given by 

p(t) = e^p( 0). 
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Figure 3.10: The vector p(0) is rotated by an angle 9 about the axis w, to p{9). 


Since t and 9 are interchangeable, the equation above can also be written 

p{9) = e^ e p( 0). 

We now derive a closed-form expression for e^ e . Here we make use of the 
Cayley-Hamilton Theorem. First, the characteristic polynomial associated with 
the 3x3 matrix [d>] is given by 

p(s) = det(s/ — [w]) = s 3 + s. 


The Cayley-Hamilton Theorem then implies [a)] 3 + [u>] = 0, or 


M = - w . 


Let us now expand the matrix exponential e^ e in series form. Replacing [w] 3 
by — [w], [w] 4 by — [w] 2 , [w] 5 by — [w] 3 = [w], and so on, we obtain 


= I + [u}0 + [^ + [*] 3 ^ + ... 

r In 03 05 

" /+, ^3! + 5!~' 


9 2 9 4 9 6 

2\ ~ 4! + 6! 


Now recall the series expansions for sin0 and cos0: 


sin 9 
cos 9 



0 5 

5l 



9 4 

4! 


The exponential therefore simplifies to the following: 

Proposition 3.9. Given a vector Cj9 £ R 3 , such that 9 is any scalar and Cj £ R 3 
is a unit vector, 

Rot(a),0) = = I + sin0 [d>] + (1 — cos0)[d)] 2 £ SO( 3). 


This formula provides the matrix exponential of [<2>\9 £ so(3). 


(3.51) 
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This formula is also known as Rodrigues’ formula for rotations. 

We have shown how to use the matrix exponential to construct a rotation 
matrix from a rotation axis Co and an angle 9. Further, the quantity e^ 9 p 
amounts to rotating p £ R 3 about the fixed-frame axis Co by an angle 9. Sim¬ 
ilarly, considering that a rotation matrix R consists of three column vectors, 
the rotation matrix R! = e^ e R = Rot(w, 0)R is the orientation achieved by 
rotating R by 9 about the axis Co in the fixed frame. Reversing the order of 
matrix multiplication, R" = Re^ e = R Rot(d>, 9) is the orientation achieved by 
rotating R by 9 about Co in the body frame. 

Example 3.1. The frame {b} in Figure 3.11 is obtained by rotating from 
an initial orientation aligned with the fixed frame {s} about a unit axis Co = 
(0, 0.866, 0.5) t by an angle of 9 = 30° = 0.524 rad. The rotation matrix repre¬ 
sentation of {b} can be calculated as 

R = e [c,]e 

= I + sin 9{Co\ + (1 — cos 9) [Co] 2 



0 

-0.5 

0.866 ' 


0 

-0.5 

0.866 ' 

7 + 0.5 

0.5 

0 

0 

+ 0.134 

0.5 

0 

0 


-0.866 

0 

0 


-0.866 

0 

0 


0.866 —0.250 0.433 
0.250 0.967 0.058 

-0.433 0.058 0.900 


The frame {b} can be represented by R or by its exponential coordinates Co = 
(0,0.866, 0.5) t and 9 = 0.524 rad, i.e., Co9 = (0,0.453,0.262) r . 

If {b} is then rotated by —9 = —0.524 rad about the same fixed-frame axis 
w, i.e., 

R' = e~W e R, 

then we would find R' = I, as expected; the frame has rotated back to the 
identity (aligned with the {s} frame). On the other hand, if {b} were to be 
rotated by —9 about Co in the body frame (this axis is different from Co in the 
fixed frame), the new orientation would not be aligned with {s}: 

R" = Re-W 9 ± I. 


Our next task is to show that for any rotation matrix R £ SO( 3), one can 
always find a unit vector Co and scalar 9 such that R = e^ 9 . 

3.2.3.3 Matrix Logarithm of Rotations 

If Co9 £ R 3 represents the exponential coordinates of a rotation matrix R , then 
the skew-symmetric matrix [Co9\ = [Cj]9 is the matrix logarithm of the rotation 
R. The matrix logarithm is the inverse of the matrix exponential. Just as the 
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Figure 3.11: The frame {b} is obtained by rotating from {s} by 9 = 30° about 
w = (0,0.866, 0.5) t . 


matrix exponential “integrates” the matrix representation of an angular velocity 
[u}6 £ so(3) for one second to give an orientation R £ SO(3 ), the matrix 
logarithm “differentiates” an R £ 50(3) to find the matrix representation of 
a constant angular velocity \CS\6 £ so(3) which, if integrated for one second, 
rotates a frame from I to R. In other words, 


exp : [Cj\6 £ so(3) —> R £ 50(3) 
log : R £ 50(3) —> [CJ\9 £ so(3) 


To derive the matrix logarithm, let us expand each of the entries for in 
Equation (3.51), 


cg+cof(l-cg) 0)10)2(1 - eg) - W3S0 Wi0)3(1 - eg) + u> 2 sg 
o)iW 2 (l - eg) + U} 3 Sg Cg+0J$(1-Cg ) L0 2 UJ 3 (1 - Cg) - oqSfl 

0)10)3(1 - ce) - W 2 Sg U) 2 Cj 3 (l - Cg) + WiSg Ce+w|(l-Cg) 


( 3 . 52 ) 


where co = ( 0 ) 1 , w 2 , o) 3 ) r , and we use the shorthand notation sg = sin0 and 
eg = cos 6. Setting the above equal to the given R £ 50(3) and subtracting the 
transpose from both sides leads to the following: 


r 3 2 — r 23 = 2 oq sin 6 

v 13 — r 3 i = 2 d ) 2 sin 6 

- 02 = 2 o) 3 sin 9. 

Therefore, as long as sin0 ^ 0 (or equivalently, 9 is not an integer multiple of 
7r), we can write 


oq = 
o) 2 = 
0)3 = 


2sin0 

1 

2sin0 

1 


(r 32 - r 23 ) 
(ri 3 - r 3 i) 
(r 2 i - ri 2 ). 


2sin0 

The above equations can also be expressed in skew-symmetric matrix form as 


0 -0)3 o) 2 

0)3 0 —0)1 

—0)2 0)1 0 


1 


2 sin 9 


(.R - R t ) . 


( 3 . 53 ) 
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Recall that ui represents the axis of rotation for the given R. Because of the 
sin 9 term in the denominator, [w] is not well defined if 9 is an integer multiple 
of 7r. 5 We address this situation next, but for now let us assume this is not 
the case and find an expression for 9. Setting R equal to (3.52) and taking the 
trace of both sides (recall that the trace of a matrix is the sum of its diagonal 
entries), 

tr R = m + r 22 + r 33 = 1 + 2 cos 6*. (3.54) 

The above follows since uif + cuf + wf = 1. For any 9 satisfying 1 + 2 cos 9 = tr R 
such that 9 is not an integer multiple of 7r, R can be expressed as the exponential 
with [w] as given in Equation (3.53). 

Let us now return to the case 9 = kir 7 where k is some integer. When k is 
an even integer, regardless of u>, we have rotated back to R = I, so the vector 
6j is undefined. When k is an odd integer (corresponding to 9 = ±7r,±37r,..., 
which in turn implies tr R = —1), the exponential formula (3.51) simplifies to 

R=e [a] ‘* = I + 2 [ w ] 2 . (3.55) 

The three diagonal terms of Equation (3.55) can be manipulated to 

u, i = ±^±l, i = 1,2,3. (3.56) 

The off-diagonal terms lead to the following three equations: 

2 w 1 u) 2 = r 12 

2w 2 d> 3 = r 23 (3.57) 

2 wid> 3 = ri 3 , 

From Equation (3.55) we also know that R must be symmetric: r 12 = r 2 1 , 

r 23 = r 32 , ri 3 = r 3 i. Both Equations (3.56) and (3.57) may be necessary to 
obtain a solution for u>. Once a solution ui has been found, then R = e ^ e , 
where 9 = ±7r, ±37t, .... 

From the above it can be seen that solutions for 9 exist at 27t intervals. If 
we restrict 9 to the interval [0,7r], then the following algorithm can be used to 
compute the matrix logarithm of the rotation matrix R £ 50(3). 

Algorithm: Given R £ 50(3), find a 9 £ [0,7r] and a unit rotation axis 
lu £ R 3 , ||w = 11|, such that = R. The vector ui9 £ M 3 comprises the 
exponential coordinates for R and the skew-symmetric matrix [Cj\9 £ so(3) is a 
matrix logarithm of R. 

(i) If R = /, then 9 = 0 and Cj is undefined. 

5 A singularity such as this is unavoidable for any three-parameter representation of rota¬ 
tion. Euler angles and roll-pitch-yaw angles suffer similar singularities. 
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Figure 3.12: S0(3) as a solid ball of radius ir. 


(ii) If tr R = —1, then 6 = n. Set Cj to any of the following three vectors that 
is a feasible solution: 


or 


or 


1 

y/2(l + r 33 ) 


r 13 

T23 

1 + ^33 


1 

\/2(l + r 2 /) 


r 12 

1 + r 2 2 

^32 


1 

\/2(l + rn) 


1 + r n 
^21 
r-31 


(iii) Otherwise 6 = cos 1 ^ ^ 1 S [0,7r) and 

-AR-R t ). 


1 


2 sin 8 


(3.58) 


(3.59) 


(3.60) 


(3.61) 


Since every i? S 50(3) satisfies one of the three cases in the algorithm, for 
every R there exists a set of exponential coordinates u)9. 

The formula for the logarithm suggests a picture of the rotation group 50(3) 
as a solid ball of radius n (see Figure 3.12): given a point r £ IR 3 in this solid 
ball, let Cj = 7”/|M| be the unit axis in the direction from the origin to r and 
and 9 = ||r|| be the distance from the origin to r, so that r = CiO. The rotation 
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matrix corresponding to r can then be regarded as a rotation about the axis Cj 
by an angle 9. For any R £ 50(3) such that tr R ^ —1, there exists a unique r 
in the interior of the solid ball such that = R. In the event that tr R = —1, 
log A. is given by two antipodal points on the surface of this solid ball. That 
is, if there exists some r such that R = e M with ||r|| = 7r, then R = e^-~ r ^ also 
holds; both r and — r correspond to the same rotation R. 


3.3 Rigid-Body Motions and Twists 

In this section we derive representations for rigid-body configurations and ve¬ 
locities that extend, but otherwise are analogous to, those in Section 3.2 for 
rotations and angular velocities. In particular, the homogeneous transforma¬ 
tion matrix T is analogous to the rotation matrix R; a screw axis S is analogous 
to a rotation axis w; a twist V can be expressed as S8 and is analogous to an 
angular velocity ui = 6 j8; and exponential coordinates S9 £ R 6 for rigid-body 
motions are analogous to exponential coordinates uj9 £ R 3 for rotations. 


3.3.1 Homogeneous Transformation Matrices 


We now consider representations for the combined orientation and position of 
a rigid body. A natural choice would be to use a rotation matrix R £ 5*0(3) 
to represent the orientation of {b} in {s} and a vector p £ R 3 to represent the 
origin of {b} in {s}. Rather than identifying R and p separately, we package 
them into a single matrix as follows. 

Definition 3.4. The Special Euclidean Group SE( 3), also known as the 
group of rigid-body motions or homogeneous transformations in R 3 , is 

the set of all 4 x 4 real matrices T of the form 





rn 

r\2 

ri 3 

Pi 

R 

P 


T21 

T22 

^23 

P2 

0 

1 


T31 

r$ 2 

r; 33 

P3 




0 

0 

0 

1 


where R £ 50(3) and p £ R 3 is a column vector. 

An element T £ SE{ 3) will sometimes be denoted ( R,p ). We begin this 
section by establishing some basic properties of SE( 3), and explaining why we 
package R and p into this matrix form. 

Many of the robotic mechanisms we have encountered thus far are planar. 
With planar rigid-body motions in mind, we make the following definition: 

Definition 3.5. The Special Euclidean Group SE( 2) is the set of all 3 x 3 

real matrices T of the form 


R p 
0 1 5 


(3.63) 


where R £ 50(2), p £ R 2 , and 0 denotes a row vector of two zeros. 
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A matrix T £ SE( 2) is always of the form 


ru 

ri2 

Pi 


cosf? 

— sind 

Pi 

T21 

T22 

P2 

= 

sin 9 

cos 9 

P2 

0 

0 

1 


0 

0 

1 


where 9 £ [0, 2tt). 


3.3.1.1 Properties of Transformation Matrices 

We now list some basic properties of transformation matrices, which can be 
proven by calculation. First, the identity I is a trivial example of a transforma¬ 
tion matrix. The first three properties confirm that SE(3) is a group. 

Proposition 3.10. The inverse of a transformation matrix T £ SE( 3) is also 
a transformation matrix, and has the following form 


1 _ 

' R 

P 

-1 

■ r t 

- R t p ' 


0 

1 


0 

1 


(3.64) 


Proposition 3.11. The product of two transformation matrices is also a trans¬ 
formation matrix. 

Proposition 3.12. Multiplication of transformation matrices is associative, 
(TiT 2 )Ts = TilTfTs), but generally not commutative, T\Ti ^ TfTi. 

Before stating the next proposition, we note that just as in Section 3.1, 
it is often useful to calculate the quantity Rx + p, where x £ R 3 and (R,p) 
represents T. If we append a ‘1’ to a;, making it a four-dimensional vector, this 
computation can be performed as a single matrix multiplication: 


X 


' R 

p 


X 


Rx + p 

1 


0 

1 


1 


1 


The vector (x T , 1) T is the representation of x in homogeneous coordinates, 
and accordingly T £ SE( 3) is called a homogenous transformation. When, by 
an abuse of notation, we write Tx , we mean Rx + p. 

Proposition 3.13. Given T = ( R,p ) £ SE(3) and x,y £ M 3 , the following 
hold: 

(i) \\Tx — Ty\\ = ||x — y\\, where || • || denotes the standard Euclidean norm in 
M 3 , i.e., ||x|| = V x T x. 

(ii) (Tx — Tz, Ty — Tz) = (x — z,y — z) for all z £ R 3 , where (•, •) denotes the 
standard Euclidean inner product wR 3 , i.e., (x,y) = x T y. 



















3.3. Rigid-Body Motions and Twists 


79 



Figure 3.13: Three reference frames in space, and a point v that can be repre¬ 
sented in {b} as Vb = (0,0,1.5) T . 


In Proposition 3.13, T is regarded as a transformation on points in R 3 , i.e., 
T transforms a point x to Tx. The first property then asserts that T pre¬ 
serves distances, while the second asserts that T preserves angles. Specifically, 
if x,y,z € R 3 represent the three vertices of a triangle, then the triangle formed 
by the transformed vertices {Tx,Ty,Tz} has the same set of lengths and angles 
as those of the triangle {x, y, z} (the two triangles are said to be isometric). One 
can easily imagine taking {x,y,zj to be the points on a rigid body, in which 
case {Tx,Ty,Tz} represents a displaced version of the rigid body. It is in this 
sense that SE( 3) can be identified with the rigid-body motions. 

3.3.1.2 Uses of Transformation Matrices 

As with rotation matrices, there are three major uses for a transformation matrix 
T: 

(i) Represent the configuration (position and orientation) of a rigid body. 

(ii) Change the reference frame in which a vector or frame is represented. 

(iii) Displace a vector or frame. 

In the first use, T is thought of as representing the configuration of a frame; in 
the second and third uses, T is thought of as an operator that acts to change 
the reference frame or to move a vector or a frame. 

To illustrate these uses, we refer to the three reference frames {a}, {b}, 
and {c}, and the point v, in Figure 3.13. These frames are chosen so that the 
alignment of their axes is clear, allowing visual confirmation of calculations. 

Representing a configuration. The fixed frame {s} is coincident with {a}, 
and the frames {a}, {b}, and {c}, represented by T sa = ( R sa ,Psa ), Tsb = 
( Rsb,Psb), Tsc = (RsaPsc), respectively, can be expressed relative to {s} by the 
rotations 


' 1 

0 

0 ' 


' 0 

0 

1 ■ 


' -1 

0 

0 ' 

0 

1 

0 

5 Rsb - 

0 

-1 

0 

1 Rsc — 

0 

0 

1 

0 

0 

1 


1 

0 

0 


0 

1 

0 
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and the location of the origin of each frame relative to {s} can be written 



1 

o 


1 

o 

1_ 


1 

1-1 

1_ 

Psa = 

0 

: Psb = 

1 

to 

; Psc — 

1 


1 

o 


1 

o 

_ 1 


1 

o 

_ 1 


Since {a} is collocated with {s}, the transformation matrix T sa constructed from 
( R S a,Psa ) is the identity matrix. 

Any frame can be expressed relative to any other frame, not just {s}; for 
example, Tf, c = (Rb c ,Pbc) represents {b} relative to {c}: 


0 

1 

0 ' 


o ' 

0 

0 

-1 

i Pbc — 

-3 

1 - 

1 

I— 1 

0 

1 

o 


1 

1— 1 

1 _ 


It can also be shown, using Proposition 3.10, that 


T de = T. 


-l 

ed 


for any two frames {d} and {e}. 


Changing the reference frame of a vector or a frame. By a subscript 
cancellation rule analogous to that for rotations, for any three reference frames 
{a}, {b}, and {c}, and any vector v expressed in {b} as Vb, 


rri rji rji r ti rri 

J-ab+bc — — L ac 

TabVb = = V a , 

where v a is the vector v expressed in {a}. 


Displacing (rotating and translating) a vector or a frame. A transfor¬ 
mation matrix T, viewed as the pair ( R,p ) = (Rot(u), 0),p), can act on a frame 
T s b by rotating it by 0 about an axis u and translating it by p. Whether we 
pre-multiply or post-multiply T s b by the operator T determines whether the Cj 
axis and p are interpreted in the fixed frame {s} or the body frame {b}: 


T s b' = TT sb = 


T s b" = T sb T = 


R p 


' R 

sb Psb 


RRsb 

Rp sb + p 

0 1 



0 1 


0 

1 

Psb 



R p 


RsbR 

RsbP + Psb 

0 

l 



0 1 


0 

1 


(fixed frame) 

(3.66) 

(body frame). 

(3.67) 


The fixed-frame transformation (pre-multiplication by T ) can be interpreted as 
first rotating the {b} frame by 6 about an axis Cj in the {s} frame (this rotation 
will cause the origin of {b} to move if it is not coincident with the origin of 
{s}), then translating it by p in the {s} frame to get the (b 7 } frame. The 
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Figure 3.14: Fixed-frame and body-frame transformations corresponding to Cj = 
(0,0,1) T , 0 = 90°, and p = (0,2,0) T . (Left) The frame {b} rotated by 90° 
about z s and then translated by two units in y s , resulting in the new frame 
{b 7 }. (Right) The frame {b} translated by two units in y b and then rotated by 
90° about the z axis of the body frame, resulting in the new frame {b"}. 


body-frame transformation (post-multiplication by T) can be interpreted as first 
translating {b} by p considered to be in the {b} frame, then rotating about Cj 
in this new body frame (this does not move the origin of the frame) to get {b ,, |. 
Fixed-frame and body-frame transformations are illustrated in Figure 3.14 for 
a transformation T with Cj = (0, 0,1) T , 9 = 90°, and p = (0, 2, 0) T , yielding 


T = (Rot(w, 9),p) 


0-100 
1 0 0 2 

0 0 10 

0 0 0 1 


Beginning with the frame {b} represented by 


T s h = 


0 0 10 
0 - 10-2 
1 0 0 0 ’ 

0 0 0 1 


the new frame {b 7 } achieved by a fixed-frame transformation TT sb and the new 
frame {b"} achieved by a body-frame transformation T sb T are 


' 0 

1 

0 

2 ' 


0 

0 

1 

0 ' 

0 

0 

1 

2 

, T sb T = T sb n = 

-1 

0 

0 

-4 

1 

0 

0 

0 

0 

-1 

0 

0 

0 

0 

0 

1 


0 

0 

0 

1 


TT s b = T sb , = 
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Figure 3.15: Assignment of reference frames. 


Example 3.2. Figure 3.15 shows a robot arm mounted on a wheeled mobile 
platform, and a camera fixed to the ceiling. Frames {b} and {c} are respectively 
attached to the wheeled platform and the end-effector of the robot arm, and 
frame {d} is attached to the camera. A fixed frame {a} has been established, 
and the robot must pick up the object with body frame {e}. Suppose that 
the transformations Tdb and Td e can be calculated from measurements obtained 
with the camera. The transformation Tb c can be calculated using the arm’s 
joint angle measurements. The transformation T a d is assumed to be known in 
advance. Suppose these known transformations are given as follows: 


Tdb — 


Tde = 


T a d = 


Tbc 


0 

0 

1 

0 


0 0 -1 250 

0-1 0 -150 

-1 0 0 200 

0 0 0 1 

0 0 -1 300 ' 

0-1 0 100 
-10 0 120 
0 0 0 1 _ 

0 0 -1 400 ' 

0-1 0 50 

-1 0 0 300 

0 0 0 1 

-1/V2 -1/V2 30 

l/y/2 -1/V2 -40 

0 0 25 

0 0 1 


In order to calculate how to move the robot arm to pick up the object, the 
configuration of the object relative to the robot hand, T ce , must be determined. 
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We know that 


T a bT bc T ce 


-L ad-L de ? 


where the only quantity besides T ce not given to us directly is T ab . However, 
since T ab = T^T^b, we can determine T ce as follows: 


( T ad T db T bc ) 1 T ad T de . 


From the given transformations, 


-L ad-L de 


TadTdbTbc 


(TadTdbTbc) 1 


1 0 0 280 
0 1 0 -50 
0 0 1 0 
0 0 0 1 

0 —I/a/2 — 1/a/2 230 

0 l / v /2 — 1 / a /2 160 

1 0 0 75 

0 0 0 1 


0 0 1 -75 

-1/V2 1 / a /2 0 70/V2 

-1//2 -l/y/2 0 390/\/2 

0 0 0 1 


from which T ce is evaluated to be 


0 0 1 -75 

— 1 / a /2 1 / a /2 0 - 2 m / V 2 

—1/a/2 — 1/a/2 0 130/a/2 

0 0 0 1 


3.3.2 Twists 

We now consider both the linear and angular velocity of a moving frame. As 
before, denote by {s} and {b} the fixed (space) and moving (body) frames, 
respectively, and let 


T sb (t) = T(t) 


R(t) p(t) 
o l 


(3.68) 


denote the homogeneous transformation of {b} as seen from {s} (to keep the 
notation uncluttered, for the time being we write T instead of the usual T sb ). 

In Section 3.2.2 we discovered that pre- or post-multiplying R by i? -1 re¬ 
sults in a skew-symmetric representation of the angular velocity vector, either 
in fixed or body frame coordinates. One might reasonably ask if a similar prop¬ 
erty carries over to T, i.e., whether T~ lr t and TT~ l carry similar physical 
interpretations. 
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Let us first see what happens when we pre-multiply T by T 1 



R T -R t p 

R p 



0 1 

0 0 


= 

' R T R R t p ' 
0 0 


(3.69) 

= 

M v b 

0 0 ' 


(3.70) 


Recall that R T R = [w;,] is just the skew-symmetric matrix representation of the 
angular velocity expressed in {b} coordinates. Also, p is the linear velocity of 
the origin of {b} expressed in the fixed frame {s}, and R T p = v b is this linear 
velocity expressed in the frame {b}. Putting these two observations together, 
we can conclude that T~ lr T represents the linear and angular velocity of the 
moving frame relative to the stationary frame {b} currently aligned with the 
moving frame. 

The previous calculation of T _1 T suggests that it is reasonable to merge 
u>b and Vb into a single six-dimensional velocity vector. We define the spatial 
velocity in the body frame, or simply the body twist 6 , to be 


V b 


UJ b 

Vb 


eM 6 . 


(3.71) 


Just as it is convenient to have a skew-symmetric matrix representation of an 
angular velocity vector, it is convenient to have a matrix representation of a 
twist, as shown in Equation (3.70). We overload the [■] notation, writing 


T~ X T = [V fc ] 


[w b ] v b 

0 0 


€ se(3), 


(3.72) 


where [w b ] 6 so(3) and v b £ R 3 . The set of all 4 x 4 matrices of this form is 
called se(3), the matrix representation of velocities associated with the rigid- 
body configurations SE(3). 7 

Now that we have a physical interpretation for T~ lr T, let us evaluate TT _1 : 


f T -1 



r r t 

—R T p 


o 

1 


RR t p — RR T p 
0 0 

[w s ] v s 

0 0 ■ 


(3.73) 


6 The term “twist” has been used in different ways in the mechanisms and screw theory 
literature. In robotics, however, it is common to use the term to refer to a spatial velocity. 
We adopt this usage to minimize verbiage, e.g., “spatial velocity in the body frame” vs. “body 
twist.” 

7 se(3) is called the Lie algebra of the Lie group SE(S). It consists of all possible T when 
T = I. 
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Figure 3.16: Physical interpretation of v s . The initial (solid line) and displaced 
(dotted line) configurations of a rigid body. 


Observe that the skew-symmetric matrix [w s ] = RR T is the angular velocity 
expressed in fixed frame coordinates, but that v s = p — RR T p is not the linear 
velocity of the body frame origin expressed in the fixed frame (that quantity 
would simply be p). If we write v s as 

v s = p - u s x p = p +u s x (-p), (3-74) 


the physical meaning of v s can now be inferred: imagining an infinitely large 
moving body, v s is the instantaneous velocity of the point on this body currently 
at the fixed frame origin, expressed in the fixed frame (see Figure 3.16). 

As we did for w b and v b , we assemble u s and v s into a six-dimensional twist, 


V s 


u s 

V a 


eK 6 , [V s ] 


[W a ] V s 

0 0 


= TT~ 1 e se(3), 


(3.75) 


where [V s ] is the 4x4 matrix representation of V s . We call V s the spatial 
velocity in the space frame, or simply the spatial twist. 

If we regard the moving body as being infinitely large, there is an appealing 
and natural symmetry between 14 = (u s ,v s ) and Vb = ( u>b,Vb ): 

(i) u>b is the angular velocity expressed in {b}, and u s is the angular velocity 
expressed in {s}; and 

(ii) Vb is the linear velocity of a point at the origin of {b} expressed in {b}, 
and v s is the linear velocity of a point at the origin of {s} expressed in 
{s}. 

Vb can be obtained from 14 as follows: 


[V b ] = T~ lr t 

= T _1 [14] T. 


(3.76) 


Going the other way, 


[V s }=T[V b \ T~ l . 


( 3 . 77 ) 
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Writing out the terms of Equation (3.77), we get 

_ R[iOb\R T -R[u>b\R T P + Rvb 

0 0 


which, using R[uj\R t = [i?w] (Proposition 3.5) and [uj]p = — [p]u> for p, oj £ K 3 , 
can be manipulated into the following relation between Vj, and V s : 


w s 


R 

0 ' 


u b 

V s 


[p]R 

R 


y b 


Because the 6x6 matrix pre-multiplying Vb is useful for changing the frame 
of reference for twists and wrenches, as we will see shortly, we give it its own 
name. 


Definition 3.6. Given T = ( R,p ) £ SE(3), its adjoint representation [Ad-r] 
is 


[Ad-r] 


R 0 
[p\R R 


e r 6x6 . 


For any V £ R 6 , the adjoint map associated with T is 


V = [Ad T ]V, 


also sometimes written as 

V' = Ad T (V). 

In terms of the matrix form [V] £ se(3) of V £ K 6 , 

[V}=T[V]T~ 1 . 

The adjoint map satisfies the following properties, verifiable by direct calcu¬ 
lation: 

Proposition 3.14. Let Ti,T 2 6 SE{ 3), and V = ( to,v ). Then 

Ad^ (Adr 2 (V)) = Ad^TaC^) or [AdrJfAdTjV = [AdTjTjV- (3.78) 

Also, for any T £ SE(3) the following holds: 

[Adr]" 1 = [Ad T -i], (3.79) 

The second property follows from the first by choosing Tj = T -1 and T 2 = T, 
so that 

Ad T -i (Ad T (V)) = Ad T -i T (V) = Ad/(V) = V. (3.80) 
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3.3.2.1 Summary of Results on Twists 

The main results on twists derived thus far are summarized in the following 
proposition: 

Proposition 3.15. Given a fixed (space) frame {s} and a body frame {b}, let 
T s b(t) G SE( 3) be differentiable, where 


T s b{t) — 


Then 


T~ lr f sb = [H] = 


R(t) p(t ) 

0 1 

M v b 

0 0 


G se(3) 


is the matrix representation of the body twist, and 


tbT - 1 = [V s ] = 


0 0 


G se(3) 


(3.81) 

(3.82) 

(3.83) 


is the matrix representation of the spatial twist. The twists V s and V b are 
related by 


V s = 
V b = 


LO s 


R 0 ' 


Wfc 

Vs 


[p]R R 


v b 

UJ b 


R T 

0 


= [ Ad T s JVfc 


(3.84) 


v b 


—R T [p\ r 1 


= [AdrjVs. ( 3 . 85 ) 


Similarly, for any two frames {a} and {b}, a twist represented as V a in {a} is 
related to the representation V b in {b} by 


V a = [Ad T jV b , Vb = [Adrjv 0 . 


Again analogous to angular velocities, it is important to realize that for a 
given twist, its fixed-frame representation V s does not depend on the choice of 
the body frame {b}, and its body-frame representation V b does not depend on 
the choice of the fixed frame {s}. 


Example 3.3. Figure 3.17 shows a top view of a car with a single front wheel 
driving on a plane. The Zb-axis of the body frame {b} is into the page and the 
z s -axis of the fixed frame {s} is out of the page. The angle of the front wheel 
of the car causes the car’s motion to be a pure angular velocity w = 2 rad/s 
about an axis out of the page, at the point r in the plane. Inspecting the figure, 
we can write r as r s = (2, —1,0) T or r b = (2,—1.4, 0) T ; w as w 5 = (0,0,2) T or 
ujb = (0,0,—2) t ; and T sb as 


T<,h = 


-10 0 4 

0 1 0 0.4 

0 0-1 0 

0 0 0 1 


Rsb 

0 


Psb 

1 
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y S j 


{S} 



Figure 3.17: The twist corresponding to the instantaneous motion of the chassis 
of a three-wheeled vehicle can be visualized as an angular velocity w about the 
point r. 


From the figure and simple geometry, we get 

v s =ui s x (—r s ) = r s x lo s = (-2, -4, 0) T 
v b =uj b x (~r b ) = r b x uj b = (2.8,4,0) T 

to get the twists V s and V b : 


V s = 


UJ S 

V s 


0 ■ 




0 ' 

0 




0 

2 

-2 

II 

> 

u b 

Vb 

= 

-2 

2.8 

-4 




4 

1 - 

o 

1 _ 




1 

o 


To confirm these results, try calculating V s = [Ad^J V b . 


3 . 3 . 2.2 The Screw Interpretation of a Twist 

Just as an angular velocity oj can be viewed as ui9, where Cj is the unit rotation 
axis and 8 is the rate of rotation about that axis, a twist V can be interpreted 
as a screw axis S and a velocity 9 about the screw axis. 

A screw axis represents the familiar motion of a screw: rotation about the 
axis while also translating along the axis. One representation of a screw axis 
S is the collection { q , s, h}, where q € K 3 is any point on the axis; s is a unit 
vector in the direction of the axis; and h is the screw pitch, which defines the 
ratio of the linear velocity along the screw axis to the angular velocity 8 about 
the screw axis (Figure 3.18). 

Using Figure 3.18 and geometry, we can write the twist V = (ui,v) corre¬ 
sponding to an angular velocity 8 about S (represented by {q, s, h }) as 


UJ 


§8 

V 


—§8 x q + hsd 
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Figure 3.18: A screw axis S represented by a point q , a unit direction s, and a 
pitch h. 


Note that the linear velocity v is the sum of two terms: one due to translation 
along the screw axis, hsO , and one due to the linear motion at the origin induced 
by rotation about the axis, — §0 x q. The first term is in the direction of s, while 
the second term is in the plane orthogonal to s. It is not hard to show that, for 
any V = (w, v) where w/0, there exists an equivalent screw axis {q 7 s, h} and 
velocity 9 , where s = w/||w[|, 9 = ||w||, h = Cj t v/9 7 and q is chosen so that the 
term — s9 x q provides the portion of v orthogonal to the screw axis. 

If ui = 0, then the pitch h of the screw is infinite. So s is chosen as i’/||u||, 
and 9 is interpreted as the linear velocity ||w|| along s. 

Instead of representing the screw axis S using the cumbersome collection 
{< 7 , s, h}, with the possibility that h may be infinite and the non-uniqueness of 
q (any q along the screw axis may be used), we instead define the screw axis 
S using a normalized version of any twist V = (w, v) corresponding to motion 
along the screw: 

(i) If w ^ 0: S = V/||w|| = (w/||w||, i>/||w||). The screw axis S is simply 
V normalized by the length of the angular velocity vector. The angular 
velocity about the screw axis is 9 = ||w||, such that S9 = V. 

(ii) If u = 0: S = V/||v|| = (0, w/||u||). The screw axis S is simply V normal¬ 
ized by the length of the linear velocity vector. The linear velocity along 
the screw axis is 9 = ||u||, such that S9 = V. 

This leads to the following definition of a “unit” (normalized) screw axis: 
Definition 3.7. For a given reference frame, a screw axis S is written 


5 = 


w 

V 


where either (i) ||w|| = 1 or (ii) u> = 0 and ||i;|| = 1. If (i) ||w|| = 1, then 
v = —<jj x q + hu ;, where q is a point on the axis of the screw and h is the pitch of 
the screw [h = 0 for a pure rotation about the screw axis). If (ii) ||w|| = 0 and 
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|M| = 1, the pitch of the screw is h = oo and the twist is a translation along 
the axis defined by v. 

The 4x4 matrix representation [5] of S is 


[5] 


r, .1 


0 

—u 3 

0J 2 

M v 

0 0 

G se(3), [w] = 

W 3 

0 




—W 2 

Wi 

0 


(3.86) 


where the bottom row of [5] consists of all zeros. 

Important: Although we use the pair (w, v) for both normalized screw axes 
(where one of ||w|| or ||u|| must be unit) and general twists (where there are no 
constraints on u> and v), their meaning should be clear from context. 

Since a screw axis S is just a normalized twist, a screw axis represented as 
S a in a frame {a} is related to the representation Sb in a frame {b} by 


S a = [AdT ai ,]«Sb, Sb = [AdT,„J5 a 


3.3.3 Exponential Coordinate Representation of Rigid-Body 
Motions 

3.3.3.1 Exponential Coordinates of Rigid-Body Motions 


In the planar example in Section 3.1, we saw that any planar rigid-body dis¬ 
placement can be achieved by rotating the rigid body about some fixed point 
in the plane (for a pure translation, this point lies at infinity). A similar re¬ 
sult also exists for spatial rigid-body displacements: called the Chasles-Mozzi 
Theorem, it states that every rigid-body displacement can be expressed as a 
displacement along a fixed screw axis S in space. 

By analogy to the exponential coordinates uj9 for rotations, we define the six¬ 
dimensional exponential coordinates of a homogeneous transformation 
T as S9 G R 6 , where S is the screw axis and 9 is the distance that must be 
traveled along/about the screw axis to take a frame from the origin I to T. If 
the pitch of the screw axis S = (u>,v) is finite, then ||w|| = 1 and 9 corresponds 
to the angle of rotation about the screw axis. If the pitch of the screw is infinite, 
then ui = 0 and ||i>|| = 1, and 9 corresponds to the linear distance traveled along 
the screw axis. 

Also by analogy to the rotations, we define a matrix exponential and matrix 
logarithm: 

exp : [«S]0 G se(3) —> T £ SE( 3) 
log : T G SE( 3) [«S]0 G se(3) 

We begin by deriving a closed-form expression for the matrix exponential 
e \ s \ s ' Expanding the matrix exponential in series form leads to 


e [5]fl 


I + [S]9 + [S] 2 ^ 

' eH® G(9)v ' 
0 1 


0 3 

+ [< + ... 

, G(9) = I9 + [to] 6 - 


9 1 

3! 


+ ... (3.87) 
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Noting the similarity between G(9) and the series definition for e^ e , it is tempt¬ 
ing to write I + G{Q)[uj\ = e^ e , and to conclude that G(9) = (e— /)[cc] _1 . 
This is wrong: [w] _1 does not exist (try computing det[w]). 

Instead we make use of the result [w] 3 = — [w] that was obtained from the 
Cayley-Hamilton Theorem. In this case G{9) can be simplified to 


G(6) 


ie + M 


e l 

2 ! 




4! 


6 1 

6 ! 


19 + (1 — cos0)[w] + ( 9 



sin0)[w] 2 . 


9 5 

5! 


(f 

7! 



2 


(3.88) 


Putting everything together, 


Proposition 3.16. Let S = (ui,v) be a screw axis. If ||u;|| = 1, then for any 
distance 9 £ R. traveled along the axis, 


e [S]* = 


e M s (jg + (i _ cos 9) [w] + (9 — sin 9) [w] 2 ) v 

0 1 


If lo = 0 and ||u|| = 1, then 


e [5]« = 


I v9 
0 1 


(3.89) 


(3.90) 


The latter result of the proposition can be verified directly from the series 
expansion of with w set to zero. 


3.3.3.2 Matrix Logarithm of Rigid-Body Motions 

The above derivation essentially provides a constructive proof of the Chasles- 
Mozzi Theorem. That is, given an arbitrary ( R,p ) £ SE( 3), one can always 
find a screw axis S = (tv,v) and a scalar 9 such that 


e lS]0 = 


R p 
0 1 


(3.91) 


In the simplest case, if R = I, then ui = 0, and the preferred choice for v is 
v = p/||p|| (this makes 9 = ||p|| the translation distance). If R is not the identity 
matrix and tr R ^ — 1, one solution is given by 


M = jL< R - RT) (3M > 

V = G~ 1 (9)p, (3.93) 


where 9 satisfies 1 + 2 cos (9 = tr R. We leave as an exercise the verification of 
the following formula for G~ 1 (9): 


G-\9) = -I--M+{- 


9 



M 


(3.94) 
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cx >3 = 1 rad/s 


(3.37,3.37) 



i i 


{c} 


{s} 



v = (3.37,-3.37) 


Figure 3.19: Two frames in a plane. 


Finally, if tr R = —1, we choose 9 = 7r, and [w] can be obtained via the matrix 
logarithm formula on SO( 3). Once [w] and 9 have been determined, v can then 
be obtained as v = G~ 1 (9)p. 

Algorithm: Given (R,p) written as T £ SE( 3), find a 9 £ [0,7r] and a screw 
axis S = (ui,v) £ R 6 such that e^ s = T. The vector S9 £ R 6 comprises the 
exponential coordinates for T and the matrix [<S]0 £ se(3) is a matrix logarithm 
of T. 

(i) If R = I, then set w = 0, v =p/||p||, and 9 = ||p||. 

(ii) If tri? = —1, then set 9 = tt, and [w] = logi? as determined by the matrix 
logarithm formula on 5*0(3) for the case tr R = —1. v is then given by 
v = G-\9)p. 

(iii) Otherwise set 9 = cos -1 f -1 ) £ [0, n) and 



(3.95) 

(3.96) 


v 


where G 1 (9) is given by Equation (3.94). 

Example 3.4. As an example, we consider the special case of planar rigid- 
body motions and examine the matrix logarithm formula on SE(2). Suppose 
the initial and final configurations of the body are respectively represented by 
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the SE( 2) matrices in Figure 3.19: 


cos 30° 

-sin 30° 

1 

sin 30° 

cos 30° 

2 

0 

0 

1 

cos 60° 

-sin 60° 

2 

sin 60° 

cos 60° 

1 

0 

0 

1 


For this example, the rigid-body displacement occurs in the x s -y s plane. The 
corresponding screw motion therefore has its screw axis in the direction of the 
z s -axis, and is of zero pitch. The screw axis S = (uj,v), expressed in {s}, is of 
the form 


oj = (0,0,w 3 ) T 

v = (ui,u 2 ,0) t . 


Using this reduced form, we seek the screw motion that displaces the frame at 
T sb to T ac , i.e., T sc = e^ e T sb , or 


"t 7 1 - 1 — pi 5 !® 

± sc ± sb — e , 


where 


[5] 


0 —w 3 v\ 

w 3 0 v 2 

0 0 0 


We can apply the matrix logarithm algorithm directly to T SC T to obtain [<S] 
(and therefore S ) and 9 as follows: 


' 0 

-1 

3.37 ' 


w 3 


1 

1 

0 

-3.37 

, 5 = 

Vi 

= 

3.37 

0 

0 

0 


v 2 


-3.37 


9 = 7 r/6 rad (or 30°). 


The value of S means that the constant screw axis, expressed in the fixed frame 
{s}, is represented by an angular velocity of 1 rad/s about z s and a linear velocity 
of a point currently at the origin of {s} of (3.37,-3.37), expressed in the {s} 
frame. 

Alternatively, we can observe that the displacement is not a pure translation— 
T s b and T sc have rotation components that differ by an angle of 30°—and quickly 
determine that 9 = 30° and cc 3 = 1. We can also graphically determine the point 
q = (q x ,9y) in the x s -y s plane that the screw axis must pass through; for our 
example this point is given by q = (3.37,3.37). 


3.4 Wrenches 

Consider a linear force f acting on a rigid body at a point r. Defining a refer¬ 
ence frame {a}, the point r can be represented as r a el 3 , the force f can be 
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Figure 3.20: Relation between a wrench represented as JF a and Tb- 


represented as f a £l 3 , and this force creates a torque or moment m a G R 3 in 
the {a} frame: 

m a = r a x f a . 

Note that the point of application of the force along the line of action of the 
force is immaterial. 

Just as with twists, we can merge the moment and force into a single six¬ 
dimensional spatial force, or wrench, expressed in the {a} frame, F a : 


T a 


m a 

fa 


(3.97) 


If more than one wrench acts on a rigid body, the total wrench on the body 
is simply the vector sum of the individual wrenches, provided the wrenches are 
expressed in the same frame. A wrench with zero linear component is called a 

pure moment. 

A wrench in the {a} frame can be represented in another frame {b} if Xf, 0 
is known (Figure 3.20). One way to derive the relationship between T a and 
is to derive the appropriate transformations between the individual force and 
moment vectors based on techniques we have already used. 

A simpler and more insightful way to derive the relationship between J~ a and 
Tb is to (1) use the results we have already derived relating the representations 
V Q and Vb of the same twist, and (2) use the fact that the power generated (or 
dissipated) by an (J", V) pair must be the same regardless of the frame they are 
represented in. (Imagine if we could create power simply by changing our choice 
of a reference frame!) Recall that the dot product of a force and a velocity is 
power, and power is a coordinate-independent quantity. Because of this, we 
know 

VlT b = V T a T a . (3.98) 

From Proposition 3.15 we know that V a = [Adr a jVf,, and therefore Equa¬ 
tion (3.98) can be rewritten as 

V^T b = ([Ad T jV b ) T J- a 
= V b T [Ad T J T .F a . 
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Since this must hold for all 14, this simplifies to 



F b = [Ad T J T .F a . 

(3.99) 

Similarly, 

Fa = [AdT^] 11 ^. 

(3.100) 

Proposition 3.17. Given 
F b , the two representations 

a wrench F, represented in {a} as F a 
are related by 

and in {&} as 

F b 

= Ad£jF a ) = [Ad Tab ] T T a 

(3.101) 

F a 

= Ad^ ba (F b ) = [Ad Tb J T F b . 

(3.102) 


3.5 Summary 

The following table succinctly summarizes some of the key concepts from the 
chapter, as well as the parallelism between rotations and rigid-body motions. 
For more details, consult the appropriate section of the chapter. 
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Rotations 

R £ SO( 3) : 3 x 3 matrices satisfying 
R t R = I, det It = 1 


R- 1 = R t 


change of coord frame: 
RabRbc — R'<ic ■ RabPb — Pa 
rotating a frame {b}: 

R = Rot(w, 9) 

R s b’ = RR sb - rotate 9 about io s = Cu 


R s b" = R s bR■ rotate 9 about Cbb = Co 


Rigid-Body Motions 

T £ SE( 3) : 4 x 4 matrices 

r = \ R p 
[ ° i J ’ 

where R £ SO(3),p £ R 3 

T _i _ r r t -r t p 

[ 0 1 

change of coord frame: 

TabTbc = T ac , T ab p b = p a 

displacing a frame {b}: 

T Rot(d), 9) p 

[ 0 1 

T s b’ = TT s b‘. rotate 9 about Cb s = Co 
(moves {b} origin), translate p in {s} 
T s b" = T s bT: translate p in {b}, 
rotate 9 about Co in new body frame 


unit rotation axis is Co £ 
where ||w|| = 1 


“unit” screw axis is S = 


LO 

V 


where either (i) ||u;|| = 1 or 
(ii) lo = 0 and ||uj| = 1 


for a screw axis {q, s, h} with finite h , 
5 = 


UJ 


s 

V 


—s x q + hs 


angular velocity can be written lo = Cod 


twist can be written V = S8 


for any 3-vector, e.g., lo £ 


for V = 


LO 

V 



0 

-lo 3 

102 



r r 1 i 

L0} = 

lo 3 

0 

-LOi 

£ so(3) 

[V] = 

M v 

0 0 


—L02 

LOi 

0 





identities: for io,x £ R 3 ,R £ SO( 3): 
M = -M T , [io\x = - [x\lo , 
MN = (MM) t ,RMR t = [Rw] 


£ se(3) 


(the pair (w, v ) can be a twist V 
or a “unit” screw axis S, 
depending on the context) 


RR 1 = [w s ], R 1 R = [lo {,] 


TT- 1 = [V s ], T- 1 T=[V b ] 


[Ad r ] = 


3x6 


R 0 
[p\R R 
identities: [Ady] -1 = [Ady-i], 
[Ad Tl ][Ad T2 ] = [Ad Tl T 2 ] 


change of coord frame: 
COa = RabCobi ^a = RabXLb 


change of coord frame: 

S a = [AdT a J5h, V a = [AdT ai ,]V;, 


continued... 
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Rotations (cont.) 


Rigid-Body Motions (cont.) 


CjO G M 3 are exp coords for R G 50(3) S9 G R 6 are exp coords for T G SE( 3) 


exp : [Cj}9 g so ( 3 ) —R G 50 ( 3 ) 
R = Rot (a), 9) = e W e = 

I + sin 9[Cj] + (1 — cos 9) [w] 2 


exp : [ S}9 G se(3) gTg SE( 3) 


T = e^ e = 


, l *]0 * 

0 1 
where * = 


(19 + (1 — cos 9) [w] + (9 — sin 9) [w] 2 )i> 


log : R G 50(3) —> \Cj]9 G so ( 3 ) 
algorithm in Section 3.2.3.3 

log : T G SE( 3) [S}9 G se(3) 

algorithm in Section 3.3.3.2 

moment change of coord frame: 
m a = Rabm b 

wrench change of coord frame: 

E a = (' m a ,fa ) = [Ad T ba ] T Eb 


3.6 Software 

The following functions are included in the software distribution accompanying 
the book. The code below is in MATLAB format, but the code is available 
in other languages. For more details on the software, consult the code and its 
do cument at ion. 

invR = Rotlnv(R) 

Computes the inverse of the rotation matrix R. 
so3mat = VecToso3(omg) 

Returns the 3x3 skew-symmetric matrix corresponding to omg. 
omg = so3ToVec(so3mat) 

Returns the 3-vector corresponding to the 3x3 skew-symmetric matrix so3mat. 
[omghat,theta] = AxisAng3(expc3) 

Extracts the rotation axis Cj and rotation amount 9 from the 3-vector CjO of 
exponential coordinates for rotation, expc3. 

R = MatrixExp3(expc3) 

Computes the rotation matrix corresponding to a 3-vector of exponential co¬ 
ordinates for rotation. (Note: This function takes exponential coordinates as 
input, not an so(3) matrix as implied by the function’s name.) 

expc3 = MatrixLog3(R) 

Computes the 3-vector of exponential coordinates corresponding to the matrix 
logarithm of R. (Note: This function returns exponential coordinates, not an 
so(3) matrix as implied by the function’s name.) 

T = RpToTrans(R,p) 

Builds the homogeneous transformation matrix T corresponding to a rotation 
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matrix R £ 5*0(3) and a position vector p £ R 3 . 

[R,p] = TransToRp(T) 

Extracts the rotation matrix and position vector from a homogeneous transfor¬ 
mation matrix T. 

invT = TransInv(T) 

Computes the inverse of a homogeneous transformation matrix T. 
se3mat = VecTose3(V) 

Returns the se(3) matrix corresponding to a 6-vector twist V. 

V = se3ToVec(se3mat) 

Returns the 6-vector twist corresponding to an se(3) matrix se3mat. 

AdT = Adjoint(T) 

Computes the 6x6 adjoint representation [Adr] of the homogeneous transfor¬ 
mation matrix T. 

S = ScrewToAxis(q,s,h) 

Returns a normalized screw axis representation 5 of a screw described by a unit 
vector s in the direction of the screw axis, located at the point q, with pitch h. 

[S,theta] = AxisAng(expc6) 

Extracts the normalized screw axis S and distance traveled along the screw 6 
from the 6-vector of exponential coordinates SO. 

T = MatrixExp6(expc6) 

Computes the homogeneous transformation matrix corresponding to a 6-vector 
SO of exponential coordinates for rigid-body motion. (Note: This function 
takes exponential coordinates as input, not an se(3) matrix as implied by the 
function’s name.) 

expc6 = MatrixLog(T) 

Computes the 6-vector of exponential coordinates corresponding to the matrix 
logarithm of T. (Note: This function returns exponential coordinates, not an 
se(3) matrix as implied by the function’s name.) 

3.7 Notes and References 

More detailed coverage of the various parametrizations of 50(3) can be found in, 
e.g., [96] and the references cited there. The treatment of the matrix exponential 
representation for screw motions is based on the work of Brockett [14]; a more 
mathematically detailed discussion can be found in [90]. Classical screw theory 
is presented in its original form in R. Ball’s treatise [4], More modern (algebraic 
and geometric) treatments can be found in, e.g., Bottema and Roth [13], Angeles 
[1], and McCarthy [86]. 
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3.8 Exercises 


1. In terms of the x s -y s -z s coordinates of a fixed space frame {s}, the frame {a} 
has an x a -axis pointing in the direction (0, 0,1) and a y a -axis pointing in the 
direction (—1,0,0), and the frame {b} has an Xb-axis pointing in the direction 
(1, 0,0) and a y b -axis pointing in the direction (0, 0, —1). 

(a) Give your best hand drawing of the three frames. Draw them at different 
locations so they are easy to see. 

(b) Write the rotation matrices R sa and R sb . 

(c) Given R s b, how do you calculate R~ b without using a matrix inverse? Write 
R~ b and verify its correctness with your drawing. 

(d) Given R sa and R sb , how do you calculate R ab (again no matrix inverses)? 
Compute the answer and verify its correctness with your drawing. 

(e) Let R = R sb be considered as a transformation operator consisting of a 
rotation about x by —90°. Calculate Ri = R sa R, and think of R sa as a repre¬ 
sentation of an orientation, R as a rotation of R sa , and R\ as the new orientation 
after performing the rotation. Does the new orientation R\ correspond to rotat¬ 
ing R sa by —90° about the world-fixed x s -axis or the body-fixed x a -axis? Now 
calculate i ?2 = RR S a- Does the new orientation i ?2 correspond to rotating R sa 
by —90° about the world-fixed x s -axis or the body-fixed x a -axis? 

(f) Use R s b to change the representation of the point pb = (1, 2, 3) T (in {b} 
coordinates) to {s} coordinates. 

(g) Choose a point p represented by p s = (1, 2,3) T in {s} coordinates. Calculate 
p' = R s bPs and p" = Rj b p s - For each operation, should the result be interpreted 
as changing coordinates (from the {s} frame to {b}) without moving the point 
p, or as moving the location of the point without changing the reference frame 
of the representation? 

(h) An angular velocity w is represented in {s} as uj s = (3, 2,1) T . What is its 
representation w a ? 

(h) By hand, calculate the matrix logarithm [Cj\0 of R sa . (You may verify your 
answer with software.) Extract the unit angular velocity cD and rotation amount 
9. Redraw the fixed frame {s} and in it draw cD. 

(i) Calculate the matrix exponential corresponding to the exponential coordi¬ 
nates of rotation Cj9 = (1, 2,0) T . Draw the corresponding frame relative to {s}, 
as well as the rotation axis Cj. 

2. Let p be a point whose coordinates are p = (^g, — with respect to 

the fixed frame x-y-z. Suppose p is rotated about the fixed frame x-axis by 30 
degrees, then about the fixed frame y-axis by 135 degrees, and finally about 
the fixed frame z-axis by —120 degrees. Denote the coordinates of this newly 
rotated point by p'. 

(a) What are the coordinates of p'l 
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(b) Find the rotation matrix R such that p' = Rp for the p’ you obtained in (a). 

3. Suppose pi £ R 3 and p\ £ R 3 are related by p\ = Rp t , i = 1,2,3, for some 
unknown rotation matrix R to be determined. Find, if it exists, the rotation R 
for the three input-output pairs pi K > p[: 

Pi = (\/2,0, 2) t ^ Pi = (0,2, V2) t 

P2 = ( 1,1,-If ^ p'=(i=,i=,^v / 2) T 

p 3 = (0,2V2, Of ^ p' 3 = (-V2,V2,-2) T . 


4. In this exercise you are asked to prove the property R a bRbc = Rac of Equa¬ 
tion (3.22). Define the unit axes of frames {a}, {b}, and {c} by the triplet of 
orthogonal unit vectors {x a ,y a ,z a }, {x b ,y b ,z b }, and {x c ,y c ,z c }, respectively. 
Suppose that the unit axes of frame {b} can be expressed in terms of the unit 
axes of frame {a} by 


x b = rnx a + r 2i y a + r 3 iz a 
y b = ri 2 x a + r 22 y a + r 32 z a 
Zb = ?h3X a + r 23 y a + r 33 z a . 


Similarly, suppose the unit axes of frame {c} can be expressed in terms of the 
unit axes of frame {6} by 

x c = snx b + s 2 iy b + s 3 iz b 
y c = si 2 x b + s 22 y b + s 32 z b 
Z c = Si3x b + s 23 y b + s 33 z b . 

From the above prove that R a bRbc = Rac- 

5. Find the exponential coordinates uj9 £ R 3 for the SO( 3) matrix 

'0-1 O' 

0 0 -1 . 

1 0 0 


6 . Given R = Rot(x, ^)Rot(z, 7r), find the unit vector Co and angle 9 such that 
R = eW 0 . 


7. (a) Given the rotation matrix 


R = 


0 0 1 

0-10 

1 0 0 
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find all possible values for uj £ R 3 , ||w|| = 1, and 0 £ [0, 2tt\ such that e^ s = R. 
(b) The two vectors v\ , i >2 £ R 3 are related by 

v 2 = Rv i = 6 V\ 

where ui £ R 3 has length one, and 9 £ [—7r, 7r]. Given Cj = = 

(1, 0,1) T , v 2 = (0,1,1) T , find all angles 6 that satisfy the above equation. 


8. (a) Suppose we seek the logarithm of a rotation matrix R whose trace is —1. 
From the exponential formula 

e l&]0 = / sin0[w] + (1 — cos0)[w] 2 , ||u;|| =1, 


and recalling that tr R = —1 implies 6 = 7r, the above equation simplifies to 


R = I + 2[u ] 2 


1 — 2 “I - ^ 3 ) 2Cj\Cj2 

2Cj\Cj2 1 — H - ^3) 

2wiw 2 2CC2U3 1 


2Cj\Cj3 

2£j2&3, 

2^1 + Col) 


Using the fact that u 2 + u! + wf = 1, is it correct to conclude that 


tDi 


rn + 1 


L 02 


T22 + 1 


w 3 


^33 + 1 


is also a solution? 

(c) Using the fact that [d>] 3 = — [w], the identity R = / + 2[d)] 2 can also be 
written in the alternative form 


R-I = 2[w] 2 
[6j}(R-I) = 2 [w] 3 = —2 [w] 

[w](ii + J) = 0. 


The resulting equation is a system of three linear equations in (Cj\, Cj 2i w 3 ). What 
is the relation between the solution to this linear system and the logarithm of 


R1 


9. Exploiting all of the known properties of rotation matrices, determine the 
minimum number of arithmetic operations (multiplication and division, addi¬ 
tion and subtraction) required to multiply two rotation matrices. 


10 . Due to finite arithmetic precision, the numerically obtained product of 
two rotation matrices is not necessarily a rotation matrix; that is, the resulting 
rotation A may not exactly satisfy A T A = / as desired. Devise an iterative 
numerical procedure that takes an arbitrary matrix A £ R 3x3 , and produces a 
matrix R £ 50(3) that minimizes 

\\A - Rf = tr (A - R)(A - R) T . 
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11. (a) If A = [a] and B = [6] for a, b £ R 3 , then under what conditions on a 
and b does e A e B = e A+B l 

(b) If A = [V a ] and B = [14], where V a = (oj a , v a ) and 14 = (ojb, Vb) are arbitrary 
twists, then under what conditions on V 0 and Vb does e A e B = e A+B ? Try to 
give a physical description of this condition. 

(c) Under what conditions on general A,B£ R nxn does e A e B = e A+B ? 

12. (a) Given a rotation matrix A = Rot(z, a), where Rot(z,a) indicates a 
rotation about the z-axis by an angle a, find all rotation matrices R £ 50(3) 
that satisfy AR = RA. 

(b) Given rotation matrices A = Rot(z, a) and B = Rot(z, /3), with a ^ /3, find 
all rotation matrices R £ 50(3) that satisfy AR = RB. 

(c) Given arbitrary rotation matrices A, B £ 50(3), find all solutions R € 50(3) 
to the equation AR = RB. 

13. (a) Show that the three eigenvalues of a rotation matrix R £ 50(3) each 
have unit magnitude, and conclude that they can always be written {n + iu, g,— 
iv. 1 }, where p 2 + v 2 = 1. 

(b) Show that a rotation matrix R £ 50(3) can always be factored in the form 


fi v 0 

R = A -v n 0 A- 1 

0 0 1 


where A £ 50(3) and /r 2 + v 2 = 1. (Hint: Denote the eigenvector associated 
with the eigenvalue n + iv by x + iy, x, y £ R 3 , and the eigenvector associated 
with the eigenvalue 1 by z 6 R 3 . For the purposes of this problem you may 
assume that the set of vectors {x : y,zj can always be chosen to be linearly 
independent.) 

14. Given w £ R 3 , ||w|| = 1, and 9 a nonzero scalar, show that 



(16 + (1 - cos0)[w] + (9 - sin0)[o;] 2 ) 1 =- -7-^[a;] + 

u Z 


Hint: From the identity [w] 3 = — [w], express the inverse expressed as a quadratic 
matrix polynomial in [w]. 

15. (a) Given a fixed frame {0}, and a moving frame {1} in the identity 

orientation, perform the following sequence of rotations on {!}: 


(i) Rotate {1} about the {0} frame x-axis by a; call this new frame {2}. 

(ii) Rotate {2} about the {0} frame y-axis by /3; call this new frame {3}. 
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(iii) Rotate {3} about the {0} frame 2 -axis by 7 ; call this new frame {4}. 
What is the final orientation i?o 4 ? 

(b) Suppose that the third step in (a) is replaced by the following: “Rotate {3} 
about the frame {3} 2 -axis by 7 ; call this new frame {4}.” What is the final 
orientation i?o 4 ? 

(c) From the given transformations, find T ca : 


-1 

1 

/2 

0 

1 

h- 1 


1 

Ssib 

0 

1 

U2 

1 

0 

i 

1 

0 

0 


0 

1 

0 

1 

V2 

0 

V2 

0 

1 

1 

II 

-0 

1 

0 

1 

\/2 

0 

0 

0 

0 

1 


0 

0 

0 

1 . 


16. In terms of the x s -y s -z s coordinates of a fixed space frame {s}, the frame 
{a} has an x a -axis pointing in the direction ( 0 , 0 , 1 ) and a y a -axis pointing in the 
direction (—1,0,0), and the frame {b} has an Xb-axis pointing in the direction 
(1,0,0) and a y b -axis pointing in the direction (0,0,—1). The origin of {a} is 
at (3,0,0) in {s} and the origin of {b} is at (0,2,0). 

(a) Give your best hand drawing showing {a} and {b} relative to {s}. 

(b) Write the rotation matrices R sa and R sb and the transformation matrices 
T sa and T sb . 

(c) Given T sb , how do you calculate T ^ 1 without using a matrix inverse? Write 
T^ 1 and verify its correctness with your drawing. 

(d) Given T sa and T sb , how do you calculate T ab (again no matrix inverses)? 
Compute the answer and verify its correctness with your drawing. 

(e) Let T = T sb be considered as a transformation operator consisting of a rota¬ 
tion about x by —90° and a translation along y by 2 units. Calculate T\ = T sa T. 
Does T\ correspond to a rotation and translation about x s and y s , respectively 
(world-fixed transformation of T sa ), or a rotation and translation about x a and 
y a , respectively (body-fixed transformation of T sa )l Now calculate T 2 = TT sa . 
Does X 2 correspond to a body-fixed or world-fixed transformation of T sa l 

(f) Use T sb to change the representation of the point p b = (1,2,3) T (in {b} 
coordinates) to {s} coordinates. 

(g) Choose a point p represented by p s = (1, 2,3) T in {s} coordinates. Calculate 
p' = T sb p s and p" = T^pg. For each operation, should the result be interpreted 
as changing coordinates (from the {s} frame to {b}) without moving the point 
p, or as moving the location of the point without changing the reference frame 
of the representation? 

(g) A twist V is represented in {s} as V s = (3, 2,1, —1, —2, —3) T . What is its 
representation V a ? 

(h) By hand, calculate the matrix logarithm [<S]0 of T sa . (You may verify 
your answer with software.) Extract the normalized screw axis S and rotation 
amount 8. Get the {g, s, h} representation of the screw axis. Redraw the fixed 
frame {s} and in it draw S. 

(i) Calculate the matrix exponential corresponding to the exponential coordi- 
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nates of rigid-body motion S9 = (0,1, 2,3,0,0) T . Draw the corresponding frame 
relative to {s}, as well as the screw axis S. 



Figure 3.21: Four reference frames defined in a robot’s workspace. 


17. Four reference frames are shown in the robot workspace of Figure 3.21: 
the fixed frame {a}, the end-effector frame effector {b}, camera frame {c}, and 
workpiece frame {d}. 

(a) Find T a d and T c d in terms of the dimensions given in the figure. 

(b) Find T ab given that 

' 1 0 0 4 ' 

0 10 0 

lbc ~ 0 0 10 ■ 

0 0 0 1 


18. Consider a robot arm mounted on a spacecraft as shown in Figure 3.22, 
in which frames are attached to the earth {e}, satellite {s}, the spacecraft {a}, 
and the robot arm {r}, respectively. 

(a) Given T eai T ar , and T es , find T rs . 

(b) Suppose the frame {s} origin as seen from {e} is (1,1,1), and 


T P , 


-10 0 1 
0 1 0 1 

0 0-11 
0 0 0 1 
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Figure 3.22: A robot arm mounted on a spacecraft. 


Write down the coordinates of the frame {s} origin as seen from frame {r}. 



19. Two satellites are circling the earth as shown in Figure 3.23. Frames {1} 
and {2} are rigidly attached to the satellites such that their x-axes always point 
toward the earth. Satellite 1 moves at a constant speed v±, while satellite 2 
moves at a constant speed V 2 - To simplify matters assume the earth is does not 
rotate about its own axis. The fixed frame {0} is located at the center of the 
earth. Figure 3.23 shows the position of the two satellites at t = 0. 

(a) Derive frames Xbi, X 02 as a function of t. 

(b) Using your results obtain in (a), find T 21 as a function of t. 


20. Consider the high-wheel bicycle of Figure 3.24, in which the diameter of the 
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Figure 3.24: A high-wheel bicycle. 


front wheel is twice that of the rear wheel. Frames {a} and {b} are attached to 
the centers of each wheel, and frame {c} is attached to the top of the front wheel. 
Assuming the bike moves forward in the y direction, find T ac as a function of 
the front wheel’s rotation angle 9 (assume 9 = 0 at the instant shown in the 
figure). 


North Star 



Figure 3.25: Spacecraft and space station. 


21. The space station of Figure 3.25 moves in circular orbit around the earth, 
and at the same time rotates about an axis always pointing toward the north 
star. Due to an instrument malfunction, a spacecraft heading toward the space 
station is unable to locate the docking port. An earth-based ground station 
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sends the following information to the spacecraft: 


' 0 

-1 

0 

-100 ' 


0 

1 

0 

0 

300 


0 

0 

1 

500 

1 Pa = 

800 

0 

0 

0 

0 

1 



where p a is the vector p expressed in {a} frame coordinates. 

(a) From the given information, find the vector f expressed in {b} frame 
coordinates. 

(b) Determine T^ c at the instant shown in the figure. Assume here that the y 
and z axes of the {a} and {c} frames are coplanar with the docking port. 



22 . A target moves along a circular path at constant angular velocity ui rad/s 
as shown in Figure 3.26. The target is tracked by a laser mounted on a moving 
platform, rising vertically at constant speed v. Assume the laser and the plat¬ 
form start at L\ at t = 0, while the target starts at frame T\. 

(a) Derive frames Toi,Ti 2 ,To 3 as a function of t. 

(b) Using your results from part (a), derive X 23 as a function of t. 

23. Two toy cars are moving on a round table as shown in Figure 3.27. Car 1 
moves at a constant speed Vi along the circumference of the table, while car 2 
moves at a constant speed V 2 along a radius; the positions of the two vehicles 
at t = 0 are shown in the figure. 

(a) Find T 01 , T 02 as a function of t. 

(b) Find T 12 as a function of t. 
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Figure 3.27: Two toy cars on a round table. 



Figure 3.28: A robot arm with a screw joint. 


24. Figure 3.28 shows the configuration, at t = 0, of a robot arm whose first 
joint is a screw joint of pitch h = 2. The arm’s link lengths are L\ = 10, 
L /2 — — 5, and L 4 = 3. Suppose all joint angular velocities are constant, 
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with values ooi = f, W 2 = f, w 3 = — J rad/s. Find T s t,(4) £ SE( 3), i.e., the 
end-effector frame {b} £ SE( 3) relative to the fixed frame {s}, at time t = 4. 



Figure 3.29: A camera rigidly attached to a robot arm. 


25. A camera is rigidly attached to a robot arm as shown in Figure 3.29. The 
transformation X £ SE(3 ) is constant. The robot arm moves from pose 1 to 
pose 2. The transformations A € SE( 3) and B £ SE( 3) are measured and 
assumed known. 

(a) Suppose X and A are given as follows: 


' 1 

0 

0 

1 ■ 


0 

0 

1 

0 ' 

0 

1 

0 

0 

,41 = 

0 

1 

0 

1 

0 

0 

1 

0 

-1 

0 

1 

0 

0 

0 

0 

1 


0 

0 

0 

1 


What is B1 
(b) Now suppose 


Ra 

Pa 

,B = 

Rb Pb 

0 

1 

0 1 


are known and we wish to find 


X = 


Rx Px 

0 1 


Suppose Ra = and Rb = e^. What are the conditions on a £ R 3 and 
(3 £ R 3 for a solution Rx to exist? 
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(c) Now suppose we have a set of k equations 

AiX = XBi for i = l,..., fc 

Assume Ai and Bi are all known. What is the minimum number of k for which 
a unique solution exists? 


26. Draw the screw axis with q = (3, 0,0) T , s = (0, 0,1) T , and h = 2. 

27. Draw the screw axis corresponding to the twist V = (0, 2, 2,4, 0,0) T . 

28. Assume that the space frame angular velocity is u s = (1, 2, 3) T for a moving 
body with frame {b} at 


R = 


0-1 0 
0 0-1 
10 0 


in the world frame {s}. Calculate the angular velocity u>b in {b}. 


29. Two frames {a} and {b} are attached to a moving rigid body. Show that 
the twist of {a} in space frame coordinates is the same as the twist of {b} in 
space frame coordinates. 



(a) a (b) b 


Figure 3.30: A cube undergoing two different screw motions. 


30. A cube undergoes two different screw motions from frame {1} to frame {2} 
as shown in Figure 3.30. In both cases (a) and (b), the initial configuration of 
the cube is 


Toi = 


10 0 0 
0 10 1 
0 0 10 
0 0 0 1 
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(a) For each case (a) and (b), find the exponential coordinates S9 = (u,v) such 
that T 0 2 = e^ e T 0 i, where no constraints are placed on w or v. 

(b) Repeat (a), this time with the constraint that ||w|| £ [—7r,7r]. 


31. In Example 3.2 and Figure 3.15, the block the robot must pick up weighs 
1 kg, which means the robot must provide approximately 10 N of force in the 
z e direction of the block’s frame {e} (which you can assume is at the block’s 
center of mass). Express this force as a wrench in the {e} frame, J~ e . Given 
the transformation matrices in Example 3.2, express this same wrench in the 
end-effector frame {c} as T c . 

32. Given two reference frames {a} and {b} in physical space, and a fixed frame 
{o}, define the distance between frames {a} and {b} as 

dist(T oa ,T ob ) = yjd 2 + \\Pab\\ 2 

where R a b = e^ s . Suppose the fixed frame is displaced to another frame {o'}, 
and that T 0 / a = ST oa , T 0 / b = ST 0 / b for some constant S = (R s ,p s ) £ SE(3). 

(a) Evaluate dist(T D / Q , T 0 / b ) using the above distance formula. 

(b) Under what conditions on S does dist (T oa ,T ob ) = dist(T 0 / a , T 0 / b )? 


33. (a) Find the general solution to the differential equation x = Ax, where 


A = 


-2 1 
0 -1 


What happens to the solution x(t) as t —> oo? 
(b) Do the same for 


A = 


2 

1 


-1 

2 


What happens to the solution x(t ) as t —> oo? 


34. Let x £ R 2 , A £ R 2x2 , and consider the linear differential equation x{t) = 
Ax(t). Suppose that 


x(t) 



is a solution for the initial condition ir(0) = (1, —3), and 


x{t) = 



is a solution for the initial condition a;(0) = (1,1). Find A and e At . 

35. Given a differential equation of the form x = Ax + f(t), where x £ R n and 
/(f) is a given differentiable function of t. Show that the general solution can 
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be written 


(Hint: Define z(t) 


v(t) = e At x( 0 ) = f e 

Jo 


A(t—s ) 


e At x(t), and evaluate z(t).) 


f(s) ds. 


36. Referring to Appendix B, answer the following questions related to ZXZ 
Euler angles. 

(a) Derive a procedure for finding the ZXZ Euler angles of a rotation matrix. 

(b) Using the results of (a), find the ZXZ Euler angles for the following rotation 
matrix: 



2 2 ^2 

1 i i 

2 2^/2 


37. Consider a wrist mechanism with two revolute joints 9\ and 02, in which 
the end-effector frame orientation R £ SO( 3) is given by 

R = gl^d^el^d^ 


with (hi = (0,0,1) and w 2 = (0, ^=, — ^=). Determine whether the following 
orientation is reachable (that is, find, if it exists, a solution ( 81 , 62 ) for the 
following R ): 


1 

\/2 

0 

1 1 

V2 

0 

1 

0 

1 

0 

1 

V2 

V2 j 


38. Show that rotation matrices of the form 


r 11 

ri 2 

0 

T21 

T22 

^23 

^31 

^ 32 

^33 


can be parametrized using just two parameters 8 and <j> as follows: 

cos 8 — sin 8 0 

sin 9 cos <f> cos 9 cos <j> — sin <f> 

sin 9 sin (f> cos 9 sin cf> cos <f> 

What should the range of values be for 9 and <jP. 

39. Figure 3.31 shows a three degree of freedom wrist mechanism in its zero 
position (that is, with all its joints set to zero). 

(a) Express the tool frame orientation R 03 = R(a, /3, 7) as a product of three 
rotation matrices. 

(b) Find all possible angles (a,/3, 7) for the two values of 1?03 given below. If 
no solution exists, explain why in terms of the analogy between SO(3) and the 
solid ball of radius n. 
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Figure 3.31: A three degree of freedom wrist mechanism. 


(i) R03 


0 1 0 

1 0 0 

0 0-1 


(ii) R 03 = eW* , where w = ( 0 ,^=, ^=). 


40. Refer to Appendix B. 

(a) Verify the formula for obtaining the unit quaternion representation of a ro¬ 
tation R £ 50(3). 

(b) Verify the formula for obtaining the rotation matrix R given a unit quater¬ 
nion q £ S 3 . 

(c) Verify the product rule for two unit quaternions. That is, given two unit 
quaternions q,p £ S 3 corresponding respectively to the rotations R,Q £ 50(3), 
find a formula for the unit quaternion representation of the product RQ £ 
50(3). 


41. (Refer to Appendix B.) The Cayley transform of Equation (B.18) can be 
generalized to higher-order as follows: 

R = (I — [r]) k (I + [r])~ k . (3.103) 


(a) For the case k = 2, show that the rotation R corresponding to r can be 
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computed from the formula 


R = I + 4 


1 — r 1 r 


(1 + r T r) 2 (1 + r T r) 2 


W + 


(3.104) 


(b) Conversely, given a rotation matrix R , show that a vector r that satisfies 
equation (3.104) can be obtained as 


r = uj tan 


6 

4’ 


(3.105) 


where as before Cj is the unit vector corresponding to the axis of rotation for R , 
and 9 is the corresponding rotation angle. Is this solution unique? 

(c) Show that the angular velocity in the body-fixed frame obeys the following 
relation: 

f — - {(1 — r T r)I + 2[r] + 2rr T } u>. (3.106) 

(d) Explain what happens to the singularity at n that exists for the standard 
Cayley-Rodrigues parameters. Discuss the relative advantages and disadvan¬ 
tages of the modified Cayley-Rodrigues parameters, particularly as one goes to 
order k = 4 and higher. 

(e) Compare the number of arithmetic operations for multiplying two rota¬ 
tion matrices, two unit quaternions, and two Cayley-Rodrigues representations. 
Which requires fewer arithmetic operations? 


42. Among the programming languages for which the software exists, choose 
your favorite language. If it is not represented, port the Chapter 3 software to 
your favorite programming language. 


43. Write a function that returns “true” if a given 3x3 matrix is within e of 
being a rotation matrix, and “false” otherwise. It is up to you how to define 
the “distance” between a random 3x3 real matrix and the closest member of 
SO( 3). If the function returns “true,” it should also return the “nearest” matrix 
in 50(3). See, for example, Exercise 10. 

44. Write a function that returns “true” if a given 4x4 matrix is within e of 
an element of SE( 3), and “false” otherwise. 

45. Write a function that returns “true” if a given 3x3 matrix is within e of 
an element of so(3), and “false” otherwise. 

46. Write a function that returns “true” if a given 4x4 matrix is within e of 
an element of se(3), and “false” otherwise. 

47. The primary purpose of the provided software is to be easy to read and 
educational, reinforcing the concepts in the book. The code is optimized neither 
for efficiency nor robustness, nor does it do full error-checking on its inputs. 
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Familiarize yourself with all of the code in your favorite language by reading 
the functions and their comments. This should help cement your understanding 
of the material in this chapter. Then: 

(a) Rewrite one function to do full error-checking on its input, and have the 
function return a recognizable error value if the function is called with improper 
input (e.g., an argument to the function is not an element of SO( 3), SE( 3), 
so(3), or se(3), as expected). 

(b) Rewrite one function to improve computational efficiency, perhaps by using 
what you know about properties of rotation or transformation matrices. 

(c) Can you reduce the numerical sensitivity of either of the matrix logarithm 
functions? 

48. Use the provided software to write a program that allows the user to 
specify an initial configuration of a rigid body by T, a screw axis specified 
by {g, s, h}, and a total distance traveled along the screw axis 0. The program 
should calculate the final configuration T\ = e\ s ^T attained when the rigid body 
follows the screw S a distance 9 1 as well as the intermediate configurations at 
8 / 4, 0/2, and 30/4. At the initial, intermediate, and final configurations, the 
program should plot the {b} axes of the rigid body. The program should also 
calculate the screw axis <Si, and the distance 0i following Si, that takes the 
rigid body from Tf to the origin, and plot the screw axis <Si. Test the program 
with q = (0,2,0) T , s = (0, 0, 1) T , h = 2, 0 = ir, and 

1 0 0 2 ' 

0 10 0 

0 0 10 ' 

0 0 0 1 


T = 
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Chapter 4 

Forward Kinematics 


The forward kinematics of a robot refers to the calculation of the position and 
orientation of its end-effector frame from its joint values. Figure 4.1 illustrates 
the forward kinematics problem for a 3R planar open chain. Starting from the 
base link, the link lengths are Li, L 2l and L 3 . Choose a fixed frame {0} with 
origin located at the base joint as shown, and assume an end-effector frame {4} 
has been attached to the tip of the third link. The Cartesian position (a;, y ) 
and orientation <f> of the end-effector frame as a function of the joint angles 
(# 1 , 6 * 2 , 03) are then given by 

x = L\ cos 0i + L 2 cos($i + 0 2 ) + L 3 cos(0i + d 2 + 0 3 ) (4.1) 

y = Li sinfli + L 2 sin(0i + d 2 ) + L 3 sin(0i + d 2 + d 3 ) (4.2) 

= e 1 +e 2 + o 3 . (4.3) 

If one is only interested in the ( x , y) position of the end-effector, the robot’s task 
space is then taken to be the x-y plane, and the forward kinematics would consist 
of Equations (4.1)-(4.2) only. If the end-effector’s position and orientation both 
matter, the forward kinematics would consist of the three equations (4.1)-(4.3). 

While the above analysis can be done using only basic trigonometry, it is 
not difficult to imagine that for more general spatial chains, the analysis can 
become considerably more complicated. A more systematic method of deriving 
the forward kinematics might involve attaching reference frames to each of the 
links; in Figure 4.1 the three link reference frames are respectively labeled {1}, 
{2}, and {3}. The forward kinematics can then be written as a product of four 
homogeneous transformation matrices, 

Tq 4 = T 01 T 12 T 23 T 34 , (4.4) 
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Figure 4.1: Forward kinematics of a 3R planar open chain. For each frame, the 
x and y axes are shown, and the z axes are parallel and out of the page. 


where 


COS 61 

— sin 0i 

0 

0 ' 


COS 

92 

— 

sin 02 

0 

L\ 

sin6b 

0 

cos 0i 
0 

0 

1 

0 

0 

, T 12 = 

sin 

02 

0 

COS 02 
0 

0 

1 

0 

0 

0 

0 

0 

1 



0 


0 

0 

1 

cos 0 3 

— sin 0 3 

0 

L2 



' 1 

0 

0 a 3 ' 


sin 03 

cos 03 

0 

0 



0 

1 

0 

0 


0 

0 

1 

0 

, J34 = 

0 

0 

1 

0 


0 

0 

0 

1 



0 

0 

0 

1 



Observe that T34 is constant, and that each remaining Ti_i i depends only on 
the joint variable 0^. 

As an alternative to this approach, let us define M to be the position and 
orientation of frame {4} when all joint angles are set to zero (the “home” or 
“zero” position of the robot). Then 


M 


10 0 L1 + L2 + A3 

0 10 0 

0 0 1 0 

0 0 0 1 


(4.6) 


Now consider each of the revolute joint axes to be a zero-pitch screw axis. If 
0i and 62 are held at their zero position, then the screw axis corresponding to 
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rotating about joint three can be expressed in the {0} frame as 

0 
0 
1 
0 

~{Li + L2) 

0 

You should be able to confirm this by simple visual inspection of Figure 4 . 1 . 
When the arm is stretched out straight to the right at its zero configuration, 
imagine a turntable rotating with an angular velocity of W3 = 1 rad/s about 
the axis of joint 3 . The linear velocity v 3 of the point on the turntable at the 
origin of { 0 } is in the — y 0 direction at a rate of L\ + L2 units/s. Algebraically, 
v 3 = —W3 x q 3 , where q 3 is any point on the axis of joint 3 expressed in { 0 }, 
e.g., 93 = (L x + L 2 , 0,0) t . 

The screw axis £3 can be expressed in se( 3 ) matrix form as 

0-10 0 

1 0 0 ~(Lx + L 2 ) 

0 0 0 0 

0 0 0 0 


[ 5 a] = 


M v 

0 0 


s 3 = 


w 3 

V3 


Therefore, for any 83, the matrix exponential representation for screw motions 
from the previous chapter allows us to write 

Xq 4 = e [S3]S3 M (for 6>i = 0 2 = 0 ). ( 4 . 7 ) 


Now, for 8 1=0 and any fixed (but arbitrary) 83, rotation about joint two can 
be viewed as applying a screw motion to the rigid (link two)/(link three) pair, 
i.e., 

T 04 =e [S2]e2 e [S3]93 M (for 8 X = 0 ), ( 4 . 8 ) 

where [1S3] and M are as defined previously, and 


[S 2 ] 


0-10 0 
1 0 0 -Li 

0 0 0 0 

0 0 0 0 


( 4 . 9 ) 


Finally, keeping 82 and 8 3 fixed, rotation about joint one can be viewed as 
applying a screw motion to the entire rigid three-link assembly. We can therefore 
write, for arbitrary values of (81,82,83), 

T 04 = e [Si] f>i e [s 2 ]e 2e lS3]S3M, (4.10) 


where 


[5i] = 


0-100 

1 0 0 0 

0 0 0 0 

0 0 0 0 


(4.11) 
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Thus the forward kinematics can be expressed as a product of matrix exponen¬ 
tials, each corresponding to a screw motion. Note that this latter derivation of 
the forward kinematics does not make use of any link reference frames; only {0} 
and M must be defined. 

In this chapter we consider the forward kinematics of general open chains. 
One widely used representation for the forward kinematics of open chains re¬ 
lies on the Denavit-Hartenberg parameters (D-H parameters), and this 
representation makes use of Equation ( 4 . 4 ). Another representation relies on 
the Product of Exponentials (PoE) formula, which corresponds to Equa¬ 
tion ( 4 . 10 ). The advantage of the D-H representation is that it requires the 
minimum number of parameters to describe the the robot’s kinematics; for an 
n-joint robot, it uses 3 n numbers to describe the robot’s structure and n num¬ 
bers to describe the joint values. The PoE representation is not minimal (it 
requires 6n numbers to describe the n screw axes, in addition to the n joint 
values), but it has advantages over the D-H representation (e.g., no link frames 
are necessary) and it is our preferred choice of forward kinematics representa¬ 
tion. The D-H representation, and its relationship to the PoE representation, 
is described in Appendix C. 

4.1 Product of Exponentials Formula 

To use the PoE formula, it is only necessary to assign a stationary frame {s} 
(e.g., at the fixed base of the robot, or really anywhere that is convenient to 
define a reference frame) and a frame {b} at the end-effector, described by M 
when the robot is at its zero position. However, it is not uncommon to define 
a frame at each link, typically at the joint axis; these are needed for the D-H 
representation and they are useful for displaying a CAD model of the robot 
and for defining the mass properties of the link, which we will need starting in 
Chapter 8. Thus when we are defining the kinematics of an n-joint robot, we 
may either (1) minimally use the frames {s} and {b} if we are only interested in 
kinematics, or (2) refer to {s} as frame {0}, use frames {z} for i = 1... n (the 
frames for links i at joints *), and use one more frame {n + 1} (corresponding 
to {b}) at the end-effector. The frame {n + 1 } (i.e., {b}) is fixed relative to 
{n}, but it is at a more convenient location to represent the configuration of the 
end-effector. In some cases we dispense with frame {n + 1 } and simply refer to 
{n} as the end-effector frame {b}. 

4.1.1 First Formulation: Screw Axes Expressed in Base Frame 

The key concept behind the PoE formula is to regard each joint as applying a 
screw motion to all the outward links. To illustrate, consider a general spatial 
open chain like the one shown in Figure 4 . 2 , consisting of n one-dof joints that 
are connected serially. To apply the PoE formula, you must choose a fixed base 
frame and an end-effector frame attached to the last link. Place the robot in 
its zero position by setting all joint values to zero, with the direction of positive 
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Figure 4.2: Illustration of the PoE formula for an n-link spatial open chain. 


displacement (rotation for revolute joints, translation for prismatic joints) for 
each joint specified. Let M £ SE( 3) denote the configuration of the end-effector 
frame relative to the fixed base frame when the robot is in its zero position. 

Now suppose joint n is displaced to some joint value 9 n . The end-effector 
frame M then undergoes a displacement of the form 

T = e [S„]Sn Mj (4.1.2) 

where T £ SE( 3) is the new configuration of the end-effector frame, and S n = 
(u> n , v n ) is the screw axis of joint n, as expressed in the fixed base frame. If joint 
n is revolute (corresponding to a screw motion of zero pitch), then uj n £ R 3 is a 
unit vector in the positive direction of joint axis n; v n = — tu n x q n , with q n any 
arbitrary point on joint axis n as written in coordinates in the fixed base frame; 
and 9 n is the joint angle. If joint n is prismatic, then = 0; v n £ R 3 is a unit 
vector in the direction of positive translation; and 9 n represents the prismatic 
extension/retraction. 

If we assume joint n — 1 is also allowed to vary, then this has the effect of 
applying a screw motion to link n — 1 (and by extension to fink n, since link n 
is connected to link n — 1 via joint n). The end-effector frame thus undergoes a 
displacement of the form 

T = el 5 "- 11 *"- 1 (e [5 " ] *"M) . (4.13) 

Continuing with this reasoning and now allowing all the joints (9 1 ,... ,9 n ) to 
vary, it follows that 


T{9) = e [5lIei • • • e lSn-l\0n-l e lSn\0n M . 


(4.14) 
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X3 


Figure 4.3: A 3R spatial open chain. 


This is the product of exponentials formula describing the forward kinematics 
of an n-dof open chain. Specifically, we call Equation (4.14) the space form 
of the product of exponentials formula, referring to the fact that the screw axes 
are expressed in the fixed space frame. 

To summarize, to calculate the forward kinematics of an open chain using 
the space form of the PoE formula (4.14), we need the following elements: 

(i) The end-effector configuration M £ SE( 3) when the robot is at its home 
position. 

(ii) The screw axes Si.. .S n , expressed in the fixed base frame, corresponding 
to the joint motions when the robot is at its home position. 

(iii) The joint variables 9\... 6 n . 

Unlike with the D-H representation, no link reference frames need to be defined. 
Further advantages will come to light when we examine the velocity kinematics 
in the next chapter. 

4.1.2 Examples 

We now derive the forward kinematics for some common spatial open chains 
using the PoE formula. 

Example: 3R Spatial Open Chain 

Consider the 3R open chain of Figure 4.3, shown in its home position (all joint 
variables set equal to zero). Choose the fixed frame {0} and end-effector frame 
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{3} as indicated in the figure, and express all vectors and homogeneous trans¬ 
formations in terms of the fixed frame. The forward kinematics will be of the 
form 

T(0) = e l S dSi e [S2]62 e [S 3 \03 M ^ 

where M G SE( 3) is the end-effector frame configuration when the robot is in 
its zero position. By inspection M can be obtained as 

'001 Li 
0 10 0 
-10 0 -l 2 
0 0 0 1 

The screw axis Si = (w i,v\) for joint axis 1 is then given by = (0,0,1) 
and Vi = (0,0,0) (the fixed frame origin (0,0,0) is a convenient choice for the 
point qi lying on joint axis 1). To determine the screw axis S 2 for joint axis 2, 
observe that joint axis 2 points in the —y 0 axis direction, so that u 2 = ( 0 , — 1 , 0 ). 
Choose q 2 = (Ti,0,0), in which case v 2 = — w 2 xg 2 = (0,0,—Li). Finally, to 
determine the screw axis S 3 for joint axis 3, note that oj 3 = (1,0,0). Choosing 
93 = (0, 0, —L 2 ), it follows that v$ = —W 3 x 93 = (0, — L 2 , 0). 

In summary, we have the following 4x4 matrix representations for the three 
joint screw axes Si, S 2 , and S 3 : 

0 - 100 ' 

1 0 0 0 

0 0 0 0 

0 0 0 0 

0 0-1 O' 

0 0 0 0 
10 0 -Li 

0 0 0 0 

0 0 0 0 ' 

0 0-1 -L 2 
0 10 0 
0 0 0 0 

It will be more convenient to list the screw axes in the following tabular form: 


i 

UJi 

Vi 

1 

(0,0,1) 

(0,0,0) 

2 

(0, -1,0) 

(0,0,-Li) 

3 

(1,0,0) 

(0,T 2 ,0) 


[51] = 

[5 2 ] = 

[5 3 ] = 


Example: 6R Spatial Open Chain 

We now derive the forward kinematics of the 6 R open chain of Figure 4.4. Six- 
dof arms play an important role in robotics because they have the minimum 
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h---+- 1 -+---H 



number of joints allowing the end-effector to move a rigid body in all of its 
degrees of freedom, subject only to limits on the robot’s workspace. For this 
reason, six-dof robot arms are sometimes called “general purpose manipulators.” 

The zero position and the direction of positive rotation for each joint axis 
are as shown in the figure. A fixed frame {0} and end-effector frame { 6 } are 
also assigned as shown. The end-effector frame M in the zero position is then 


M = 


10 0 0 
0 1 0 3L 

0 0 10 
0 0 0 1 


(4.15) 


The screw axis for joint 1 is in the direction w 4 = (0,0,1). The most convenient 
choice for point q 3 lying on joint axis 1 is the origin, so that vi = (0, 0, 0). The 
screw axis for joint 2 is in the y direction of the fixed frame, so uj 2 = ( 0 , 1 , 0 ). 
Choosing q 2 = (0,0,0), we have v 2 = (0,0,0). The screw axis for joint 3 is 
in the direction uj 3 = (—1,0,0). Choosing q 3 = (0,0,0) leads to v 3 = (0,0,0). 
The screw axis for joint 4 is in the direction w 4 = (—1,0,0). Choosing g 4 = 
(0,A,0) leads to v 4 = (0,0, L). The screw axis for joint 5 is in the direction 
W 5 = (—1,0,0); choosing q 5 = (0,2L,0) leads to v 3 = (0,0, 2L). The screw 
axis for joint 6 is in the direction ujq = ( 0 , 1 , 0 ); choosing q@ = ( 0 , 0 , 0 ) leads 
to vq = (0,0,0). In summary, the screw axes Si = (uii,Vi), i = 1,... 6 are as 
follows: 


i 

UJi 

Vi 

1 

(0,0,1) 

(0,0,0) 

2 

(0,1,0) 

(0,0,0) 

3 

(-1,0,0) 

(0,0,0) 

4 

(-1,0,0) 

(0,0, L) 

5 

(-1,0,0) 

(0,0,2 L) 

6 

(0,1,0) 

(0,0,0) 
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Figure 4.5: The RRPRRR spatial open chain. 


Example: An RRPRRR Spatial Open Chain 

In this example we consider the six degree-of-freedom RRPRRR spatial open 
chain of Figure 4.5. The end-effector frame in the zero position is given by 


M = 


10 0 0 

0 10 L\ -f- L/2 

0 0 1 0 

0 0 0 1 


The screw axes Si = (uii,Vi) are listed in the following table: 


i 

LOi 

Vi 

1 

(0, 0, 1) 

(0, 0, 0) 

2 

(1, 0, 0) 

(0, 0, 0) 

3 

(0, 0 , 0) 

(0, 1, 0) 

4 

(0, 1, 0) 

(0, 0 , 0) 

5 

(1, 0, 0) 

(0, 0, -Li) 

6 

(0, 1 , 0) 

(0, 0, 0) 


Note that the third joint is prismatic, so that W 3 = 0 and V 3 is a unit vector in 
the direction of positive translation. 


Example: Universal Robots’ UR5 6R Robot Arm 

The Universal Robots UR5 6R robot arm is shown in Figure 4.6. Each joint 
is directly driven by a brushless motor combined with 100:1 zero-backlash har¬ 
monic drive gearing, which greatly increases the torque available at the joint 
while reducing its maximum speed. 

Figure 4.6 shows the screw axes ... <S>6 when the robot is at its zero posi- 
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W2 = 82 mm 



H2 = 95 mm 


392 mm 


W1 is the y s 
distance between 
the anti-parallel 
axes S1 and S5 


L1 = 425 mm 


Positive rotation about 
the axes is by the 
right-hand rule. 


Figure 4.6: Universal Robots’ UR5 6R robot arm (at its zero position, at right). 


tion. The end-effector frame {b} in the zero position is given by 

— 1 0 0 — L 2 

0 0 1 W 1 + W 2 

0 1 0 ' 

0 0 0 1 

The screw axes Si = are listed in the following table: 


i 

Ui 

Vi 

1 

(0, 0, 1) 

(0, 0, 0) 

2 

(0, 1, 0) 

0, 0) 

3 

(0, 1 , 0) 

{-Hu 0, Li) 

4 

(0, 1 , 0) 

{-Hu 0, L 1 +T 2 ) 

5 

(0, 0, -1) 

(-Wu Li + L 2 , 0) 

6 

(0, 1 , 0) 

{H 2 -Hu 0, L 1 +T 2 ) 


As an example of forward kinematics, set 62 = —7r/2 and 6*5 = 7r/2, with all 
other joint angles equal to zero. Then the configuration of the end-effector is 

T{9) = glAd^igl 52 ! e [ s ^\ s 3 e [Si\8i e \s 5 ]9 5 e [s s ]e 6 

= / e -[‘S2]7r/2j2 e [5 5 ]7r/2jj^- 
_ e -[S 2 ]7r/2 e [S 5 ]7r/2^ 

since e° = I. Evaluating, we get 


' 0 

0 

-1 

0.089 ' 


0 

1 

0 

0.708 ' 

0 

1 

0 

0 

[<S 5 ]tt/2 _ 

-1 

0 

0 

0.926 

1 

0 

0 

0.089 


0 

0 

1 

0 

0 

0 

0 

1 


0 

0 

0 

1 
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Figure 4.7: (Left) The UR5 at its home position, with the axes of joints 
2 and 5 indicated. (Right) The UR5 at joint angles 9 = (0i,...,0 6 ) = 
(0, —tt/2, 0,0, tt/2, 0). 


where the linear units are meters, and 


T(9) = e -\S2]^/2 e [S^/2 M 


0 -1 0 0.095 

1 0 0 0.109 

0 0 1 0.988 

0 0 0 1 


as shown in Figure 4.7. 

4.1.3 Second Formulation: Screw Axes Expressed in End-Effector 
Frame 

The matrix identity e M lpM = M~ 1 e p M (Proposition 3.7) can also be ex¬ 
pressed as Me M lpM = e p M. Beginning with the rightmost term of the pre¬ 
viously derived product of exponentials formula, if we repeatedly apply this 
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identity, after n iterations we obtain 


T(6) = e [Sl]ei ■ ■ ■ e [Sn]K M 

= e W«i...Me M " 1[s »] M( » 

= e [5llei • • • M e M ~ 1[Sn - l]M6n - 1 e M ~ 1[Srl]Men 
_ Me M_1|Sl,M#1 • • • e M_1 [5„-i]Me„_i e M- 1 [5n]M0„ 

= lei®"]®", (4.16) 


where each [25^] = M _1 [5,]M = [Ad M -i]iSj, i = 1,... ,n. Equation (4.16) is an 
alternative form of the product of exponentials formula, representing the joint 
axes as screw axes Bi in the end-effector (body) frame when the robot is at 
its zero position. We call Equation (4.16) the body form of the product of 
exponentials formula. 

It is worth thinking about the order of the transformations expressed in the 
space form PoE formula (Equation (4.14)) and in the body form formula (Equa¬ 
tion (4.16)). In the space form, M is first transformed by the most distal joint, 
progressively moving inward to more proximal joints. Note that the fixed space- 
frame representation of the screw axis for a more proximal joint is not affected 
by the joint displacement at a distal joint (e.g., joint three’s displacement does 
not affect joint two’s screw axis representation in the space frame). In the body 
form, M is first transformed by the first joint, progressively moving outward to 
more distal joints. The body-frame representation of the screw axis for a more 
distal joint is not affected by the joint displacement at a proximal joint (e.g., 
joint two’s displacement does not affect joint three’s screw axis representation 
in the body frame.) Therefore, it makes sense that we need only determine 
the screw axes at the robot’s zero position: any Si is unaffected by more distal 
transformations, and any B t is unaffected by more proximal transformations. 


Example: 6R Spatial Open Chain 

We now express the forward kinematics for the same 6R open chain of Figure 4.4 
in the second form 

T(0) = M e^ 1 ^ 1 e^ 2 ^ 2 ■ ■ ■ e^ 6 ^ 6 . 

Assume the same fixed and end-effector frames and zero position as the previous 
example. M is still the same as in Equation (4.15), obtained as the end-effector 
frame as seen from the fixed frame with the chain in its zero position. The 
screw axis for each joint axis, however, is now expressed with respect to the 
end-effector frame in its zero position: 


i 

UJi 

Vi 

i 

(0,0,1) 

(—3L, 0,0) 

2 

(0,1,0) 

(0,0,0) 

3 

(-1,0,0) 

(0,0,-3L) 

4 

(-1,0,0) 

(0,0, -2L) 

5 

(-1,0,0) 

ST 

i 

o' 

cT 

6 

(0,1,0) 

(0,0,0) 
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y s and y b axes are aligned and out of the page. 
Axes 1,2, and 3 intersect at the origin of {s}. 
Axes 5, 6, and 7 intersect at a common point 
60 mm from the origin of {b}. Axes 1, 3, 5, 
and 7 are aligned with z s , and axes 2, 4, and 6 are 
out of the page at the zero configuration. Positive 
rotation about the axes is by the right-hand rule. 


Figure 4.8: Barrett Technology’s WAM 7R robot arm at its zero configuration 
(right). 


Example: Barrett Technology’s WAM 7R Robot Arm 

Barrett Technology’s WAM 7R robot arm is shown in Figure 4.8. The extra 
(seventh) joint means that the robot is redundant for the task of positioning its 
end-effector frame in SE( 3) in general, for a given end-effector configuration 
in the robot’s workspace, there is a one-dimensional set of joint variables in 
the robot’s seven-dimensional joint space that achieves that configuration. This 
extra degree of freedom can be used for obstacle avoidance or to optimize some 
objective function, such as minimizing the motor power needed to hold the robot 
at that configuration. 

Also, some of the joints of the WAM are driven by motors placed at the 
base of the robot, reducing the robot’s moving mass. Torques are transferred 
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from the motors to the joints by cables winding around drums at the joints and 
motors. Because the moving mass is reduced, the motor torque requirements 
are decreased, allowing lower (cable) gearing ratios and high speeds. This design 
is in contrast to the design of the UR5, where the motor and harmonic drive 
gearing for each joint are directly at the joint. 

Figure 4.8 illustrates the WAM’s end-effector frame screw axes B 1 ...B 7 
when the robot is at its zero position. The end-effector frame {b} in the zero 
position is given by 


10 0 0 

0 10 0 

0 0 1 L\ + L 2 + L 3 

0 0 0 1 


The screw axes Bi = (wj,Uj) are listed in the following table: 


i 

Wi 

Vi 

1 

( 0, 0, 1) 

(0, 0, 0) 

2 

( 0, 1, 0) 

(Li + L2 + I/3, 0, 0) 

3 

( 0, 0, 1) 

(0, 0, 0) 

4 

( 0, 1, 0) 

(L 2 + L 3 , 0, Wr) 

5 

( 0, 0, 1) 

(0, 0, 0) 

6 

( 0, 1, 0) 

(L 3l 0, 0) 

7 

( 0, 0, 1) 

(0, 0, 0) 


Figure 4.9 shows the WAM arm with 62 = 45°, $4 = —45°, 9q = —90°, and 
all other joint angles equal to zero, giving 


T(0) = = 


0 0-1 0.3157 

0 10 0 

1 0 0 0.6571 

0 0 0 1 


4.2 The Universal Robot Description Format 

The Universal Robot Description Format (URDF) is an XML (extensible 
Markup Language) file format used by the Robot Operating System (ROS) to 
describe the kinematics, inertial properties, and link geometry of robots. A 
URDF file describes the joints and links of a robot: 

• Joints. Joints connect two links: a parent link and a child link. A 
few of the possible joint types include prismatic, revolute (including joint 
limits), continuous (revolute without joint limits), and fixed (a virtual 
joint that does not permit any motion). Each joint has an origin frame 
that defines the position and orientation of the child link frame relative 
to the parent link frame when the joint variable is zero. The origin is 
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Figure 4.9: (Left) The WAM at its home configuration, with the axes of 
joints 2, 4, and 6 indicated. (Right) The WAM at 6 = = 

(0,7t/4,0,—tt/4,0,—tt/2,0). 


on the joint’s axis. Each joint has an axis x-y-z unit vector, expressed in 
the child link’s frame, in the direction of positive rotation for a revolute 
joint or positive translation for a prismatic joint. 

• Links. While joints fully describe the kinematics of a robot, links define 
the mass properties of the links. These are necessary starting in Chapter 8, 
when we begin to study the dynamics of robots. The elements of a link 
include its mass; an origin frame that defines the position and orientation 
of a frame at the link’s center of mass relative to the link’s joint frame 
described above; and an inertia matrix, relative to the link’s center of 
mass frame, specified by the six elements of the inertia matrix on or above 
the diagonal. (As we will see in Chapter 8, the inertia matrix for a rigid 
body is a 3 x 3 symmetric positive-definite matrix. Since the inertia matrix 
is symmetric, it is only necessary to define the terms on and above the 
diagonal.) 

Note that most links have two frames rigidly attached: a first frame at the joint 
(defined by the joint element that connects the link to its parent) and a second 
frame at the link’s center of mass (defined by the link element, relative to the 
first frame). 

A URDF file can represent any robot with a tree structure. This includes 
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J5 L5 


J1 LI 
_ 



base link, 


LO 




parent: J1 ’s parent link, LO 
child: Jl’s child link, LI 
origin : the x-y-z and roll-pitch-yaw 

of the LI frame relative to the 
LO frame when J1 is zero 
axis : the x-y-z unit vector along the 

rotation axis in the LI frame 


mass: 
origin: 




k inertia: 


Iink5’s mass 

the x-y-z and roll-pitch-yaw 
of a frame at the center of 
mass of L5, relative to L5 
frame 

six unique entries of 
inertia matrix in CM frame 


Figure 4.10: A five-link robot represented as a tree, where nodes of the tree are 
the links and the edges of the tree are the joints. 


serial chain robot arms and robot hands, but not a Stewart platform or other 
mechanisms with closed loops. An example robot with a tree structure is shown 
in Figure 4.10. 

The orientation of a frame {b} relative to a frame {a} is represented using 
roll-pitch-yaw coordinates: first, roll about the fixed x a -axis; then pitch about 
the fixed y a -axis; then yaw about the fixed z a -axis. 

The kinematics and mass properties of the UR5 robot arm (Figure 4.11) are 
defined in the URDF file below, which demonstrates the syntax of the joint’s el¬ 
ements parent, child, origin, and axis, and the link’s elements mass, origin, 
and inertia. A URDF requires a frame defined at every joint, so we define 
frames {1} to {6}, in addition to the fixed base frame {0} (i.e., {s}) and the end- 
effector frame {7} (i.e., {b}). Figure 4.11 gives the extra information needed to 
fully write the URDF. 

Although the joint types in the URDF are defined as “continuous,” the UR5 
joints do in fact have joint limits; they are omitted here for simplicity. 

The UR5 URDF file (kinematics and inertial properties only). 


<?xml version="l.0" ?> 
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Figure 4.11: The orientations of the frames {s} (also written {0}), {b} (also 
written {7}), and {1} through {6} are illustrated on the translucent UR5. The 
frames {s} and {1} are aligned with each other; frames {2} and {3} are aligned 
with each other; and frames {4}, {5}, and {6} are aligned with each other. 
Therefore, only the axes of frames {s}, {2}, {4}, and {b} are labeled. Just 
below the image of the robot is a skeleton indicating how the frames are offset 
from each other, including the distance and the direction (expressed in the {s} 
frame). 


<robot name="ur5"> 

<!— ********** KINEMATIC PROPERTIES (JOINTS) ********** —> 
<joint name="world_joint" type="fixed"> 

<parent link="world"/> 

<child link="base_link"/> 

<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/> 

</joint> 

<joint name="jointl" type="continuous"> 

<parent link="base_link"/> 

<child link="linkl"/> 

<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.089159"/> 

<axis xyz="0 0 l"/> 
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</joint> 

<joint name="joint2" type="continuous"> 

<parent link="linkl"/> 

<child link="link2"/> 

<origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.13585 0.0"/> 
<axis xyz="0 1 0"/> 

</joint> 

<joint name="joint3" type="continuous"> 

<parent link="link2"/> 

<child link="link3"/> 

<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.1197 0.425"/> 

<axis xyz="0 1 0"/> 

</joint> 

<joint name="joint4" type="continuous"> 

<parent link="link3"/> 

<child link="link4"/> 

<origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.0 0.39225"/> 
<axis xyz="0 1 0"/> 

</joint> 

<joint name="joint5" type="contimious"> 

<parent link="link4"/> 

<child link="link5"/> 

<origin rpy="0.0 0.0 0.0" xyz="0.0 0.093 0.0"/> 

<axis xyz="0 0 l"/> 

</joint> 

<joint name="joint6" type="continuous"> 

<parent link="link5"/> 

<child link="link6"/> 

<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/> 

<axis xyz="0 1 0"/> 

</joint> 

<joint name="ee_joint" type="fixed"> 

<origin rpy="-l.570796325 0 0" xyz="0 0.0823 0"/> 

<parent link="link6"/> 

<child link="ee_link"/> 

</joint> 

<!— ********** INERTIAL PROPERTIES (LINKS) ********** —> 
<link name="world"/> 

<link name="base_link"> 

<inertial> 

<mass value="4.0"/> 

<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/> 

<inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" 

iyy="0.00443333156" iyz="0.0" izz="0.0072"/> 

</inertial> 

</link> 

<link name="linkl"> 

<inertial> 

<mass value="3.7"/> 

Corigin rpy="0 0 0" xyz="0.0 0.0 0.0"/> 

<inertia ixx="0.010267495893" ixy="0.0" ixz="0.0" 

iyy="0.010267495893" iyz="0.0" izz="0.00666"/> 

</inertial> 

</link> 

<link name="link2"> 


<inertial> 
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<mass value="8.393"/> 

<origin rpy="0 0 0" xyz="0.0 
<inertia ixx="0.22689067591" 
iyy="0.22689067591" 

</inertial> 

</link> 

<link name="link3"> 

<inertial> 

<mass value="2.275"/> 

<origin rpy="0 0 0" xyz="0.0 
<inertia ixx="0.049443313556" 
iyy="0.049443313556" 

</inertial> 

</link> 

<link name="link4"> 

<inertial> 

<mass value="l.219"/> 

<origin rpy="0 0 0" xyz="0.0 
<inertia ixx="0.111172755531" 
iyy="0.111172755531" 

</inertial> 

</link> 

<link name="link5"> 

<inertial> 

<mass value="l.219"/> 

<origin rpy="0 0 0" xyz="0.0 
<inertia ixx="0.111172755531" 
iyy="0.111172755531" 

</inertial> 

</link> 

<link name="link6"> 

<inertial> 

<mass value="0.1879"/> 

<origin rpy="0 0 0" xyz="0.0 
<inertia ixx="0.0171364731454 
iyy="0.0171364731454 

</inertial> 

</link> 

<link name="ee_link"/> 

</robot> 


0.0 0.28"/> 

ixy="0.0" ixz="0.0" 

iyz="0.0" izz="0.0151074"/> 


0.0 0.25"/> 
ixy="0.0" ixz="0.0" 
iyz="0.0" izz="0.004095"/> 


0.0 0 . 0 "/> 
ixy="0.0" ixz="0.0" 
iyz="0.0" izz="0.21942"/> 


0.0 0.0"/> 
ixy="0.0" ixz="0.0" 
iyz="0.0" izz="0.21942"/> 


0.0 0.0"/> 

" ixy="0.0" ixz="0.0" 

" iyz="0.0" izz="0.033822"/> 


Beyond the properties described above, a URDF can describe other prop¬ 
erties of a robot, such as its visual appearance (including CAD models of the 
links) as well as simplified representations of link geometries that can be used 
for collision detection in motion planning algorithms. 


4.3 Summary 

• Given an open chain with a fixed reference frame {s} and a reference 
frame {b} attached to some point on its last link —this frame is denoted 
the end-effector frame —the forward kinematics is the mapping T{9) 
from the joint values 9 to the position and orientation of {b} in {s}. 



Forward Kinematics 


In the Denavit-Hartenberg representation, the forward kinematics of 
an open chain is described in terms of relative displacements between 
reference frames attached to each link. If the link frames are sequentially 
labeled {0},..., {n + 1 }, where {0} is the fixed frame {s}, {*} is a frame 
attached to link i at joint i (i = 1... n), and {n + 1} is the end-effector 
frame {b}, then the forward kinematics is expressed as 


7o,n+i(<?) — 2oi(^i) • • ’ T n _i : n(9 n )T ntn+ i 

where 9i denotes the joint i variable and T nt7l+ i indicates the (fixed) con¬ 
figuration of the end-effector frame in {?r}. If the end-effector frame {b} 
is chosen to be coincident with {n}, then we can dispense with the frame 
{n + 1}. 

The Denavit-Hartenberg convention requires that reference frames as¬ 
signed to each link obey a strict convention (see Appendix C). Following 
this convention, the link frame transformation between link frames 

{* — 1} and {«} can be parametrized using only four parameters, the 
Denavit-Hartenberg parameters. Three of these describe the kine¬ 
matic structure, while the fourth is the joint value. Four numbers is the 
minimum needed to represent the displacement between two link frames. 

The forward kinematics can also be expressed as the following product 
of exponentials (the space form), 

T(9) = e [Sl]f)l ■ ■ ■ e [Sn]e " M, 

where Si = (ojj, Vi ) denotes the screw axis associated with positive motion 
along joint i expressed in fixed frame coordinates {s}, 9i is the joint i 
variable, and M G SE( 3) denotes the position and orientation of the end- 
effector frame {b} when the robot is in its zero position. It is not necessary 
to define individual link frames; it is only necessary to define M and the 
screw axes Si,... ,S n . 

The product of exponentials formula can also be written in the equivalent 

body form, 

T(9) =Me Wl -"e [8A , 

where [,B;] = [AdM- 1 ]^, i = 1 ,... ,n. Bt = (u>i,Vi) is the screw axis 
corresponding to joint axis i, expressed in {b}, with the robot in its zero 
position. 

The Universal Robot Description Format (URDF) is a file format, used 
by the Robot Operating System and other software, for representing the 
kinematics, inertial properties, visual properties, and other information 
for general tree-like robot mechanisms, including serial chains. A URDF 
file includes descriptions of joints, which connect a parent link and a child 
link and fully specify the kinematics of the robot, as well as descriptions 
of links, which specify inertial properties. 
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4.4 Software 


T = FKinBody(M,Blist,thetalist) 

Computes the end-effector frame given the zero position of the end-effector M, 
the list of joint screws expressed in the end-effector frame Blist, and the list of 
joint values thetalist. 

T = FKinSpace(M,Slist,thetalist) 

Computes the end-effector frame given the zero position of the end-effector M, 
the list of joint screws expressed in the fixed space frame Slist, and the list of 
joint values thetalist. 


Notes and References 

The product of exponentials formula is first presented by Brockett in [14]. Com¬ 
putational aspects of the product of exponentials formula are also discussed in 
[95]. 
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4.5 Exercises 


1. Familiarize yourself with the functions FKinBody and FKinFixed in your 
favorite programming language. Can you make these functions more computa¬ 
tionally efficient? If so, indicate how. If not, explain why not. 





Figure 4.12: An RRRP SCARA robot for performing pick-and-place operations. 


2. The RRRP SCARA robot of Figure 4.12 is shown in its zero position. 
Determine the end-effector zero position configuration M, the screw axes Si 
in {0}, and the screw axes £>; in {b}. For £q = i\ = (-2 = 1 and the joint 
variable values 6 = (0, 7t/2, —7t/2, 1), use both the FKinFixed and the FKinBody 
functions to find the end-effector configuration T £ SE( 3). Confirm that they 
agree with each other. 

3. Determine the end-effector frame screw axes Bi for the 3R robot in Figure 4.3. 

4. Determine the end-effector frame screw axes £>,; for the RRPRRR robot in 
Figure 4.5. 

5. Determine the end-effector frame screw axes Bi for the UR5 robot in Fig¬ 
ure 4.6. 


6 . Determine the space frame screw axes Si for the WAM robot in Figure 4.8. 
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Figure 4.13: A PRRRRR spatial open chain. 


7. The PRRRRR spatial open chain of Figure 4.13 is shown in its zero position. 
Determine the end-effector zero position configuration M, the screw axes Si in 
{0}, and the screw axes B, in {b}. 



{0} y 0 w y» 


Figure 4.14: A spatial RRRRPR open chain. 


8 . The spatial RRRRPR open chain of Figure 4.14 is shown in its zero position, 
with fixed and end-effector frames chosen as shown. Determine the end-effector 
zero position configuration M, the screw axes Si in {0}, and the screw axes Bi 
in {b}. 

9. The spatial RRPPRR open chain of Figure 4.15 is shown in its zero position. 
Determine the end-effector zero position configuration M. the screw axes 5, in 
{0}, and the screw axes £>, in {b}. 


10 . The URRPR spatial open chain of Figure 4.16 is shown in its zero position. 
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Figure 4.15: A spatial RRPPRR open chain with prescribed fixed and end- 
effector frames. 


Determine the end-effector zero position configuration M, the screw axes S t in 
{ 0 }, and the screw axes £>,; in {b}. 

11. The spatial RPRRR open chain of Figure 4.17 is shown in its zero position. 
Determine the end-effector zero position configuration M. the screw axes <S, in 
{ 0 }, and the screw axes £>, in {b}. 

12. The RRPRRR spatial open chain of Figure 4.18 is shown in its zero position 
(all joints lie on the same plane). Determine the end-effector zero position 
configuration M , the screw axes S, in {0}, and the screw axes Bi in {b}. Setting 
9 ,5 = 7 r and all other joint variables to zero, find Tqq and Tqq. 

13. The spatial RRRPRR open chain of Figure 4.19 is shown in its zero position. 
Determine the end-effector zero position configuration M. the screw axes 5, in 
{ 0 }, and the screw axes in {b}. 

14. The RPH robot of Figure 4.20 is shown in its zero position. Determine the 
end-effector zero position configuration M, the screw axes Si in {s}, and the 
screw axes Bi in {b}. Use both the FKinFixed and the FKinBody functions to 
find the end-effector configuration T £ SE{ 3) when 9 = (7 t/2 , 3,7 t). Confirm 
that they agree with each other. 

15. The HRR robot in Figure 4.21 is shown in its zero position. Determine the 
end-effector zero position configuration M, the screw axes St in { 0 }, and the 
screw axes Bi in {b}. 


16. The forward kinematics of a four-dof open chain in its zero position is 
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Figure 4.16: A URRPR spatial open chain robot. 


written in the following exponential form: 
T(6) = 


Suppose the manipulator’s zero position is redefined to 
(0i, 02, ^3, ^4) = (0:1,0:2,0:3,04). 

Defining 9 'j = 9i — at,i = 1,..., 4, the forward kinematics can then be written 

T 0 4(9' 1 ,9' 2 ,e' 3 ,e' 4 ) = e [A,lle ' 1 e [A ' 2le ' 2 M , e lA,3le ' 3 e [A ' 410 ' 4 . 

Find M' and each of the A'i. 


17. Figure 4.22 shows a snake robot with end-effectors at each end. Reference 
frames {bi} and {b 2 } are attached to the two end-effectors as shown. 

(a) Suppose the end-effector 1 is grasping a tree (which ca be thought of as 
ground), and end effector 2 is free to move. Assume the robot is in its zero 
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Figure 4.17: An RPRRR spatial open chain. 



Figure 4.18: An RRPRRR spatial open chain. 


configuration. Then Tjj k £ SE(3) can be expressed in the following product 
of exponentials form: 


T bib2 = e l s dSi e [S 2 \S2 ... e [S 5 ]05 Mi 


Find 6 > 3 , 6 > 5 , and M. 

(b) Now suppose end-effector 2 is rigidly grasping a tree, and end-effector 1 is 
free to move. Then T babi £ SE( 3) can be expressed in the following product 
of exponentials form: 

— e [-A 5 ]0 5 e [^l 4]04 e [-A 3 ]e 3 ^y e [-4.2]02 e l-4l]^l 

b 2 bi 


Find A 2 ,A 4 , and N. 
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xo 


Figure 4.19: A spatial RRRPRR open chain with prescribed fixed and end- 
effector frames. 



Figure 4.20: An RPH open chain shown at its zero position. All arrows 
along/about the joint axes are drawn in the positive direction (i.e., in the di¬ 
rection of increasing joint value). The pitch of the screw joint is 0.1 m/rad, 
i.e., it advances linearly by 0.1 m for every radian rotated. The link lengths are 
L 0 = 4, Li = 3, L 2 = 2, and L 3 = 1 (figure not drawn to scale). 


18 . The two identical PUPR open chains of Figure 4.23 are shown in their zero 
position. 

(a) In terms of the given fixed frame {A} and end-effector frame {a}, the forward 
kinematics for the robot on the left (robot A) can be expressed in the following 
product of exponentials form: 

T Aa = e [Sl]9l e [S2]82 ■ ■ ■ e [S5]e5 M a . 


Find <S >2 and S 4 . 

(b) Suppose the end-effector of robot A is inserted into the end-effector of robot 
B (so that the origins of the end-effectors coincide); the two robots then form a 
single-loop closed chain. Then the configuration space of the single-loop closed 
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Figure 4.21: HRR robot. Denote the pitch of the screw joint by h. 



Figure 4.22: Snake robot 
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chain can be expressed in the form 

e -[B 5 ]^ e -[B4]<P ie -[B 3 }<i> 3e -[B 2 ]^ 2e -[B 1 ]4> le [s 1 ]9 le [s 2 ]e 2e [s 3 ]e 3e [s i ]8 ie [s 5 ]e 5 = M 

for some constant M £ SE( 3) and 8 , = {u>i,Vi),i = 1, ■ ■ • ,5. Find 83,85 and 
M. (Hint: given any A £ (e" 4 ) -1 = e~‘ 4 ). 

12 



Figure 4.23: Two PUPR open chains. 


19 . The RRPRR spatial open chain of Figure 4.24 is shown in its zero position. 

(a) The forward kinematics can be expressed in the form 

T sb = M!e [Al]ei M 2 e [A2]<>2 ■ ■ ■ M b e [M]05 . 

Find M 2 , M 3 ,A 2 , and A 3 . 

(b) Expressing the forward kinematics in the form 

T sb = e [ s d0i e [S2]02 ... e [Ss]es M, 

find M and S 3 ,... ,S 3 in terms of M 1; ..., M 5 , Ai,..., A 3 appearing in (a). 

20 . The spatial PRRPRR open chain of Figure 4.25 is shown in its zero po¬ 
sition, with space and end-effector frames chosen as shown. Derive its forward 
kinematics in the form 

To„ = e[‘ Sl ^ 1 et‘ s ' 2 ^ 2 et' s, 3 ^ 3 et' s, 4 ^ 4 e^' s ' 5 ^ 5 Met ‘ s ' 6 ^ 6 


where M G SE( 3). 
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Figure 4.24: A spatial RRPRR open chain. 



Figure 4.25: A spatial PRRPRR open chain. 


21 . (Refer to Appendix C.) For each T £ SE(3) below, find, if they exist, 
values for the four parameters (a, a, d, (/)) that satisfy 

T = Rot(x, a) Trans(x, a) Trans(z, d) Rot(z, <f>). 
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(a )T = 


(b )T = 


( c ) T = 


0 113 
10 0 0 

0 10 1 ' 

0 0 0 1 

cos (3 sin (3 0 1 

sin f3 — cos /3 0 0 

0 0 - 1-2 
0 0 0 1 

0-1 0 -1 ' 

0 0-10 
1 0 0 2 ' 

0 0 0 1 
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Chapter 5 

Velocity Kinematics and 
Statics 


In the previous chapter we saw how to calculate the robot end-effector frame’s 
position and orientation for a given set of joint positions. In this chapter we 
examine the related problem of calculating the end-effector’s twist from a given 
set of joint positions and velocities. 

Before we get to a representation of the end-effector twist as V £ R 6 starting 
in Section 5.1, let us consider the case where the end-effector configuration is 
represented by a minimal set of coordinates x £ R m and the velocity is given 
by x £ M m . In this case, the forward kinematics can be written as 

x{t) = 

where 9 £ R” is a set of joint variables. By the chain rule, the time derivative 
at time t is 

= me, 

where J{9) £ R mx " i s called the Jacobian. The Jacobian matrix represents 
the linear sensitivity of the end-effector velocity x to the joint velocity 6 , and it 
is a function of the joint variables 9. 

To provide a concrete example, consider a 2R planar open chain (left side of 
Figure 5.1) with forward kinematics given by 

x\ = Li cos 9\ + L 2 cos($i + 62 ) 
x 2 = L\ sin 9i + L 2 sin( 6 *i + 0 2 ) • 

Differentiating both sides with respect to time yields 

X\ — — L\9\ sin 9 1 — L 2 [0\ T ^ 2 ) sin($i T 9 2 ) 

x 2 = Li9i cosdi + L 2 (0i + 9 2 ) cos(0i + 9 2 ), 
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Figure 5.1: (Left) A 2R robot arm. (Right) Columns 1 and 2 of the Jacobian 
correspond to the endpoint velocity when 8 \ = 1 (and 0 2 = 0) and when 82 = 1 
(and 81 =0), respectively. 


which can be rearranged into an equation of the form x 


J{ 8 ) 8 : 



—L\ sin 81 — L 2 sin(0i + 0 2 ) 
L\ cos 81 + L 2 cos(0i + 82 ) 


— L 2 sin(0 i + 8 2 ) 
L 2 cos(0i + 82 ) 



(5.1) 


Writing the two columns of J(0) as J\{ 8 ) and J 2 (0), and the tip velocity x as 
Vtip, Equation (5.1) can be written 


Vtip — Ji(0)0i + J2 (0)02 • 


As long as Ji(0) and J 2 (0) are not collinear, it is possible to generate a tip 
velocity uti p in any arbitrary direction in the X 1 -X 2 plane by choosing appropriate 
joint velocities 8 \ and 02- Since Ji(0) and J 2 (0) depend on the joint values 0i 
and 0 2 , one may ask if there are any configurations at which Ji(0) and J 2 (0) 
become collinear. For our example the answer is yes: if 0 2 is 0° or 180°, then 
regardless of the value of 0i, J\( 8 ) and </ 2 (0) will be collinear, and the Jacobian 
J(0) becomes a singular matrix. Such configurations are called singularities; 
they are characterized by the robot tip being unable to generate velocities in 
certain directions. 

Now let’s plug in L\ = L 2 = 1 and consider the robot at two different 
nonsingular postures: 0 = (0, 7t/4) and 0 = (0, 37t/4). The Jacobians J(0) at 
these two configurations are 


A 



-0.71 -0.71 
1.71 0.71 



0 

37t/4 


-0.71 -0.71 
0.29 -0.71 


The right side of Figure 5.1 illustrates the robot at the 0 2 = 7r/4 configuration. 
Column i of the Jacobian matrix, <A(0), corresponds to the tip velocity when 
0j = 1 and the other joint velocity is zero. These tip velocities (and therefore 
Jacobian columns) are indicated in Figure 5.1. 

The Jacobian can be used to map bounds on the rotational speed of the joints 
to bounds on i>ti p , as illustrated in Figure 5.2. Instead of mapping a polygon 
of joint velocities through the Jacobian as in Figure 5.2, we could instead map 
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A 


C 


B 


0 2 ik 


D 


A 



C 


Figure 5.2: Mapping the set of possible joint velocities, represented as a square 
in the 61-62 space, through the Jacobian to find the parallelogram of possible 
end-effector velocities. The extreme points A, B, C, and D in the joint velocity 
space map to the extreme points A, B, C, and D in the end-effector velocity 
space. 

a unit circle of joint velocities in the 61-62 plane. This circle represents an 
“iso-effort” contour in the joint velocity space, where total actuator effort is 
considered to be the sum of squares of the joint velocities. This circle maps 
through the Jacobian to an ellipse in the space of tip velocities, and this ellipse 
is referred to as the manipulability ellipsoid . 1 Figure 5.3 shows examples of 
this mapping for the two different postures of the 2R arm. As the manipulator 
configuration approaches a singularity, the ellipse collapses to a line segment, 
since the ability of the tip to move in one direction is lost. 

Using the manipulability ellipsoid, one can quantify how close a given pos¬ 
ture is to a singularity. For example, we can compare the lengths of the major 
and minor principal semi-axes of the manipulability ellipsoid, respectively de¬ 
noted f'max and £ min . The closer the ellipsoid is to a circle—meaning the ratio 
fmax/fmin is small, close to 1—the more easily the tip can move in arbitrary 
directions, and thus the more removed it is from a singularity. 

The Jacobian also plays a central role in static analysis. Suppose an external 
force is applied to the robot tip. What are the joint torques required to resist 
this external force? 

This question can be answered via a conservation of power argument. As¬ 
suming there is no power loss due to joint friction, and that the robot is at 
equilibrium (no power is used to move the robot), the power measured at the 
robot’s tip must equal the power generated at the joints. Denoting the tip 
force vector generated by the robot as / t ; p and the joint torque vector by r, 


1 A two-dimensional ellipsoid, as in our example, is commonly referred to as an ellipse. 
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Figure 5.3: Manipulability ellipsoids for two different postures of the 2R planar 
open chain. 


conservation of power then requires that 

ftip v tip = T @1 


for all arbitrary joint velocities 9. Since u t i p = J(6)9 , the equality 

/tip me = r T e 

must hold for all possible 9. 2 This can only be true if 

r = J T (0)/ti P . (5.2) 

The joint torque r needed to create the tip force f t - ip is calculated from the 
equation above. 

2 Since the robot is at equilibrium, the joint velocity 0 is technically zero. This can be 
considered the limiting case as 6 approaches zero. To be more formal, we could invoke the 
“principle of virtual work” which deals with infinitesimal joint displacements instead of joint 
velocities. 
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Figure 5.4: Mapping joint torque bounds to tip force bounds. 


For our two-link planar chain example, J{9 ) is a square matrix dependent 
on 9. If the configuration 9 is not a singularity, then both J(9) and its tranpose 
are invertible, and Equation (5.2) can be written 

/tip = ((JWfrV = J~ t (9)t. (5.3) 

Using the equation above, one can now determine, under the same static equilib¬ 
rium assumption, what input torques are needed to generate a desired tip force, 
e.g., to determine the joint torques needed for the robot tip to push against 
a wall with a specified normal force. For a given posture 9 of the robot at 
equilibrium, and given a set of joint torque limits such as 

— 1 Nm <n < 1 Nm 

— 1 Nm <T 2 < 1 Nm, 

Equation (5.3) can be used to generate the set of all possible tip forces as shown 
in Figure 5.4. 

Similar to the manipulability ellipsoid, a force ellipsoid can be drawn by 
mapping a unit circle “iso-effort” contour in the T 1 -T 2 plane to an ellipsoid in the 
/ 1-/2 tip force plane via the Jacobian transpose inverse J~ T (9) (see Figure 5.5). 
This illustrates how easily the robot can generate forces in different directions. 
As evident from the manipulability and force ellipsoids, if it is easy to generate a 
tip velocity in a given direction, then it is difficult to generate force in that same 
direction, and vice-versa (Figure 5.6). In fact, for a given robot configuration, 
the principal axes of the manipulability ellipsoid and force ellipsoid are identical, 
and the lengths of the principal semi-axes of the force ellipsoid are the reciprocals 
of the lengths of the principal semi-axes of the manipulability ellipsoid. 

At a singularity, the manipulability ellipsoid collapses to a line segment. 
The force ellipsoid, on the other hand, becomes infinitely long in a direction 
orthogonal to the manipulability ellipsoid line segment (i.e., the direction of the 
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Figure 5.5: Force ellipsoids for two different postures of the 2R planar open 
chain. 


aligned links) and skinny in the orthogonal direction. Consider, for example, 
carrying a heavy suitcase with your arm. It is much easier if your arm hangs 
straight down in gravity (with your elbow fully straightened, at a singularity), 
because the force you must support passes directly through your joints, there¬ 
fore requiring no torques about the joints. Only the joint structure bears the 
load, not muscles generating torques. Unlike the manipulability ellipsoid, which 
loses dimension at a singularity and therefore its area drops to zero, the force 
ellipsoid’s area goes to infinity. (Assuming the joints can support the load!) 

In this chapter we present methods for deriving the Jacobian for general open 
chains, where the configuration of the end-effector is expressed as T £ SE( 3) 
and its velocity is expressed as a twist V in the fixed base frame or the end- 
effector body frame. We also examine how the Jacobian can be used for velocity 
and static analysis, including identifying kinematic singularities and determining 
the manipulability and force ellipsoids. Later chapters on inverse kinematics, 
motion planning, dynamics, and control make extensive use of the Jacobian and 
related notions introduced in this chapter. 
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Figure 5.6: Left column: Manipulability ellipsoids at two different arm config¬ 
urations. Right column: Force ellipsoids at the same two arm configurations. 

5.1 Manipulator Jacobian 

In the 2R planar open chain example, we saw that for any joint configuration 9, 
the tip velocity vector uti p and joint velocity vector 6 are linearly related via the 
Jacobian matrix J{9 ), i.e., i! t i p = J{9)9. The tip velocity r>ti p depends on the 
coordinates of interest for the tip, which in turn determines the specific form of 
the Jacobian. For example, in the most general case, u t i p can be taken to be the 
six-dinrensional twist of the end-effector frame, while for pure orienting devices 
like a wrist, Ut; p is usually taken to be the angular velocity of the end-effector 
frame. Other choices for uti p lead to different formulations for the Jacobian. 
We begin with the general case where Uti p is taken to be the six-dimensional 
end-effector twist expressed in the fixed frame. 

5.1.1 Space Jacobian 

In this section we derive the relationship between an open chain’s joint velocity 
vector 9 and the end-effector’s spatial twist V s . We first recall a few basic 
properties from linear algebra and linear differential equations: (i) if A, B 6 
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I" x " are both invertible, then ( AB ) _1 = B~ 1 A~ 1 -, (ii) if A £ R” xrl is constant 
and 9{t) is a scalar function of t , then jjj.e Ae = Ae Ae 9 = e AB A9; (iii) (e Ae ) _1 = 
e~ AB . 

Consider an n-link open chain whose forward kinematics is expressed in the 
following product of exponentials form: 

T(0i, ...,9 n ) = e [Sl]dl e [S2 \ 92 ■ ■ ■ e [Sn]f>n M. (5.4) 

The spatial twist is given by [V s ] = TT -1 , where 


T 


^ e [Si]9i^ ... e [Sn]0„ M + e [Si]fli ■ ■ ■ e^M + ... 

[5i]0ie [5l]91 • • • e [5 " ]e "M + e [5l]ei [S 2 ]9 2 e lS2]<>2 ■ ■ ■ e [Sn]0n M +... 


Also, 


7 1 - 1 = M - 1 e “[ 5n ] 071 • • • e - ! 51 ) 6 * 1 . 


Calculating TT 1 , 

[V«] = [Si]0i + e [Sllei [S 2 ]e“ [Sllei 0 2 + e [Sllei e [S2le2 [S 3 ]e“ [S2lS2 e- [Sll01 0 3 + .... 
The above can also be expressed in vector form by means of the adjoint mapping: 


V s = Si 0i + Adgispe! (S 2 ) 02 + Ad e [s 1 ]e le [s 2 ]e 2 (S 3 ) 03 + ■ 


(5.5) 


Observe that V s is a sum of n spatial twists of the form 

V s = V s i(0)0i + ... + V s „(0)0„, (5.6) 


where each V S j(0) = (<jJ S i(9),v S i(9)) depends explictly on the joint values 0 £ R" 
for i = 2,..., n. In matrix form, 


V s = [ Vsi(0) v s2 (0) ••• Vs„(0) ] 

= Js{9)9. 

The matrix J s (9) is said to be the Jacobian in fixed (space) frame coordinates, 
or more simply the space Jacobian. 

Definition 5.1. Let the forward kinematics of an n-link open chain be expressed 
in the following product of exponentials form: 

T = e [Sl]t>1 ■■■e [Sn]9n M. (5.8) 

The space Jacobian J s (0) £ R 6xn relates the joint rate vector 0 £ R" to the 
end-effector spatial twist V s via 



V s = J s (0)0. 


(5.9) 
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Figure 5.7: Space Jacobian for a spatial RRRP chain. 


The ith column of J s {9) is 


Vsi(O) = •A.d e [ Sl ]9 1 ___ e [s 4 _ 1 ]e < _ 1 {Si), ( 5 . 10 ) 

for i = 2,..., n, with the first column V s i(0) = <Si. □ 

To understand the physical meaning behind the columns of J s {9), observe 
that the ith column is of the form Ad^.j {Si), where T)_i = e^ 1 ! 01 • • • el 5 *- 1 ! 04 - 1 ; 
recall that Si is the screw axis describing the ith joint axis in terms of the fixed 
frame with the robot in its zero position. AdT i _ 1 (5j) is therefore the screw 
axis describing the ith joint axis after it undergoes the rigid body displacement 
T i _ 1 . Physically this is the same as moving the first i—1 joints from their zero 
position to the current values 0 1; ..., Qi-\. Therefore, the ith column V s ,(0) of 
J s {9 ) is simply the screw vector describing joint axis i, expressed in fixed frame 
coordinates, as a function of the joint variables 9\,..., 9i-\. 

In summary, the procedure for determining the columns V S i of J s {9) is similar 
to the procedure for deriving the joint screws Si in the product of exponentials 
formula e^ 1 ] 01 • • • e iSri ^ Sn M: each column V S i{9) is the screw vector describing 
joint axis i, expressed in fixed frame coordinates, but for arbitrary 9 rather than 
0 = 0 . 

Example: Space Jacobian for a Spatial RRRP Chain 

We now illustrate the procedure for finding the space Jacobian for the spatial 
RRRP chain of Figure 5 . 7 . Denote the ith column of J s {9) by V si = {oj si , v si ). 
The [Adj^J matrices are implicit in our calculations of the screw axes of the 
displaced joint axes. 

• Observe that w s i is constant and in the z s -direction: w s i = ( 0 , 0 , 1 ). Pick¬ 
ing qi to be the origin, = (0, 0,0). 
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Figure 5.8: Space Jacobian for the spatial RRPRRR chain. 

• lo s 2 is also constant in the z s -direction, so uj s2 = (0,0,1). Pick q 2 to be 
the point (L 1 C 1 , L 1 S 1 ,0), where Ci = cos0i, si = sin O-i. Then v s2 = 
—W 2 x q 2 = (L 1 S 1 , —L 1 C 1 ,0). 

• The direction of w S 3 is always fixed in the z s -direction regardless of the 
values of 6\ and 0 2 , so w S 3 = (0,0,1). Picking <73 = (L 1 C 1 + L 2 c\ 2 , T 1 S 1 + 
^ 2 Si 2 , 0 ), where C 12 = 008(01 + $ 2 ), S 12 = sin( 0 i + 6 2 ), it follows that 
^3 = (Ll$l + ^2Sl2, — L 1 C 1 — L 2 C 12 ,0). 

• Since the final joint is prismatic, w s 4 = (0, 0, 0), and the joint axis direction 
is given by v s4 = ( 0 , 0 , 1 ). 

The space Jacobian is therefore 

0 0 ' 

0 0 

1 0 

T1S1 + T2S12 0 

-L 1 C 1 — L 2 c\ 2 0 

0 1 

Example: Space Jacobian for Spatial RRPRRR Chain 

We now derive the space Jacobian for the spatial RRPRRR chain of Figure 5.8. 

The base frame is chosen as shown in the figure. 

• The first joint axis is in the direction w s i = (0,0,1). Picking q 4 = 
(0,0, Li), we get = -uji xq\ = (0,0,0). 

• The second joint axis is in the direction lu s2 = (—ci,— si,0). Picking 
q 2 = ( 0 , 0 , Li), we get v s2 = -w 2 x q 2 = (LiSi,-LiCi, 0 ). 


Jsifi) = 


0 0 

0 0 

1 1 

0 L1S1 

0 —L1C1 

0 0 
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• The third joint is prismatic, so uj s 3 = (0,0,0). The direction of the pris¬ 
matic joint axis is given by 


v s3 = Rot(z, 0i)Rot(x, -d 2 ) 

' 0 ' 
1 

— 

—Sic 2 

ClC 2 


0 


-s 2 


• Now consider the wrist portion of the chain. The wrist center is located 
at the point 


0 


0 


— (L 2 + 03)sic 2 

0 

+Rot(z, 0!)Rot(x, — 6 2 ) 

L\ + 0 3 

= 

(L 2 + 03)CiC 2 

Li 


0 


L\ — (L 2 + 9 3 )s 2 


Observe that the directions of the wrist axes depend on 64 , 0 2 , and the 
preceding wrist axes. These are 


4 


^s5 


W S 6 



' 0 ' 


— SlS 2 

Rot(z, 0i)Rot(x, — 9 2 ) 

0 

1 

— 

ClS 2 

c 2 


Rot(z, di)Rot(x, — 0 2 )Rot(z, 64 ) 


-1 

0 

0 


Rot(z, 0i)Rot(x, — 0 2 )Rot(z, 04 )Rot(x, — 0 5 ) 


—C5(SiC 2 C4 + C1S4) + SiS 2 S5 
C5(ClC 2 C4 — S1S4) — CiS 2 S5 
—S 2 C4C5 — C 2 S5 


— C1C4 + SiC 2 S4 

— S1C4 — CiC 2 S4 

S2S4 


0 

1 

0 


The space Jacobian can now be computed and written in matrix form as follows: 

j _ tUsl ^s2 0 CjJ s 4 CO s 5 ^s6 

_ [ 0 -U ] s2 x q 2 v s3 -U ) s4 x q w -w s5 x q w - w s6 x q w 

Note that we were able to obtain the entire Jacobian directly, without having 
to explicitly differentiate the forward kinematic map. 

5.1.2 Body Jacobian 

In the previous section we derived the relationship between the joint rates and 
[14] = TT -1 , the end-effector’s twist expressed in fixed frame coordinates. Here 
we derive the relationship between the joint rates and [V&] = T~ 1 T, the end- 
effector twist in end-effector frame coordinates. For this purpose it will be 
more convenient to express the forward kinematics in the alternate product of 
exponentials form: 


T(0) = Me [Bl]8l e [B2]92 ■ ■ ■ e [Bn]f>n . 


(5.11) 
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Computing T, 

f = Me^ ■ ■ ■ el ®"- 1 ]®- 1 (-el®"]®-) + Me^ • • • f± e [Bn-de n - 1)e [B n \e n + 

dt dt 

= Me 1 ® 1 ' 01 • • • e 18 " 1 ®" [B n ]9 n + Me 1 ® 1 ' 01 • • • e [B "- l1 ®"- 1 [e n _i]e [B " ] ®"0„_i + .. 
+Me [Sl101 [S^e 1821 ® 2 • • • e [8 ’* 1 ®'*0 1 . 

Also, 

j 1 - 1 = e -[B„]e„ ... e —[Si ]® 1 tv /— 1 

Evaluating T~ l T , 

[V fc ] = [B„]0„ + e- [B " le " [B n _ 1 ]e[ B “I fl »0 n _ 1 + ... 

• • • e-^l^tBilet® 3 ]® 2 • • • e^®^, 

or in vector form, 

Vb = JSn^On + Ad e -[B„]s„ {B n - 1) 9 n -1 + ... + Ad e -[6„]« n ... e -[B 2 ]» 2 (Bi) (?i. (5.12) 

Vbn V b , n -1 Vn 

Vf, can therefore be expressed as a sum of n body twists, i.e., 

Vb = Vbi(0)0i + ■ ■ • + Vb„(0)0 n , (5.13) 


where each Vbi(0) = (u)bi(9 ), Vbi(0)) depends explictly on the joint values 9 for 
i = 1,..., n — 1. In matrix form, 


Vb 


[ V 61 (0) V b2 (9) 
J b {9)9. 


V bn (9) 



(5.14) 


The matrix Jb{9) is the Jacobian in the end-effector (or body) frame coordinates, 
or more simply the body Jacobian. 


Definition 5.2. Let the forward kinematics of an n-link open chain be expressed 
in the following product of exponentials form: 

T = Me [Bll01 ---e [B “ |fl ”. (5.15) 

The body Jacobian Jb{9) G R 6xra relates the joint rate vector 9 6 R” to the 
end-effector twist Vb = (u>b, v b) via 

Vb = J b (9)9. (5.16) 


The ith column of Jb{9) is 


Vbi(9) Ad e _[ Bri ]e n __ , e -[s i+1 ]« i+1 (Bj), 
for i = n — 1,..., 1, with Vb n {9) = B n . □ 


(5.17) 
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Analogous to the columns of the space Jacobian, a similar physical interpre¬ 
tation can be given to the columns of J b (0): each column 14i(0) = (cobi(9), v b i(9)) 
of Jb{0 ) is the screw vector for joint axis *, expressed in coordinates of the end- 
effector frame rather than the fixed frame. The procedure for determining the 
columns of J b (0) is similar to the procedure for deriving the forward kinemat¬ 
ics in the product of exponentials form Me^ 1 ^ 1 • • • el 8 "] 0 "-, the only difference 
being that each of the end-effector frame joint screws Vbi{0) are expressed for 
arbitrary 0 rather than 0 = 0. 

5.1.3 Relationship between the Space and Body Jacobian 

Denoting the fixed frame by {s} and the end-effector frame by {b}, the forward 
kinematics can be written T sb (0). The twist of the end-effector frame can be 
written in terms of the fixed and end-effector frame coordinates as 

[V s ] = tbT-,, 1 
[V 6 ] = T~ b lr r s b, 

with 14 and 14 related by 14 = AdT sb {Vb) and 14 = Ad Tfcs (14). 14 and 14 are 
also related to their respective Jacobians via 

V s = J s (0)0 

V b = J b (0)9 

Equation (5.18) can therefore be written 

Ad tM) = Js(0)9. (5.20) 

Applying [Ad^J to both sides of Equation (5.20), and using the general prop¬ 
erty [Adjf][Ady] = [Ad^r] of the adjoint map, we obtain 

AdT 6f ,(Ad Ts6 (14)) = AdTbsTsiXVt,) = 14 = Ad t 6s (<4 (<?)#)• 

Since we also have 14 = J b (0)0 for all 0, it follows that J s {9) and J b (0) are 
related by 

J b (0) = Ad Tbs (J s (0)) = [Ad T J J s (0). (5.21) 

The space Jacobian can in turn be obtained from the body Jacobian via 

J s (9) = Ad Tab (J b (0)) = [Ad T J J b (9). (5.22) 

The fact that the space and body Jacobians, and space and body twists, are 
similarly related by the adjoint map should not be surprising, since each column 
of the space and body Jacobian corresponds to a twist. 

One of the important implications of Equations (5.21) and (5.22) is that 
J b (0) and J a (6) always have the same rank; this is shown explicitly in the later 
section on singularity analysis. 


(5.18) 

(5.19) 
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5.1.4 Alternative Notions of the Jacobian 

The space and body Jacobians derived above are matrices that relate joint 
rates to the twist of the end-effector. There exist alternative notions of the 
Jacobian that are based on a representation of the end-effector configuration 
using a minimum set of coordinates q. Such representations are particularly 
relevant when the task space is considered to be a subspace of SE( 3). For 
example, the configuration of the end-effector of a planar robot could be treated 
as q = (x, y, 9) G R 3 instead of as an element of SE( 2). 

When using a minimum set of coordinates, the end-effector velocity is not 
given by a twist V but by the time derivative of the coordinates q, and the Jaco¬ 
bian J a in the velocity kinematics q = J a {9)9 is sometimes called the analytic 
Jacobian, as opposed to the geometric Jacobian in space and body form, as 
described above. 

For an SE( 3) task space, a typical choice of the minimal coordinates g G R 6 
includes three coordinates for the origin of the end-effector frame in the fixed 
base frame, and three coordinates for the orientation of the end-effector frame 
in the fixed base frame. Example coordinates for the orientation include Euler 
angles (see Appendix B) and exponential coordinates for rotation. 


Example: Analytic Jacobian with Exponential Coordinates for Rotation 


In this example, we find the relationship between the geometric Jacobian J b in 
the body frame and an analytic Jacobian J a that uses exponential coordinates 
r = CjQ to represent orientation. (Recall that ||d>|| = 1 and 9 G [0,7r].) 

First, consider an open chain with n joints with body Jacobian 

V b = J b (9)9 , 

where J b (9) G R 6xrl . The angular and linear velocity components of V b = 
(ui b ,v b ) can be written explicitly as 


V b = 


LO b 

V b 


J b (9)9 


JUO) 

M9) 


0 , 


where is the 3 x n matrix corresponding to the top three rows of J b and J v 
is the 3 x n matrix corresponding to the bottom three rows of J b . 

Now suppose our minimal set of coordinates q G R 6 is given by q = (r, x), 
where x G R 3 is the position of the origin of the end-effector frame and r = 6j9 G 
R 3 is the exponential coordinate representation for the rotation. The coordinate 
time-derivative x is related to v b by a rotation to get v b in the fixed coordinates, 

x = R sb (9)v b = R sb (9)J v (9)9, 
where R sb {9) = eW = e^ e . 

The time-derivative r is related to the body angular velocity ui b by 
w b = 

A{r) = 


A[r)r 

I 1 — cos | 


r - sin r 








5.2. Statics of Open Chains 


163 


(The derivation of this formula is explored in Exercise 10.) Provided the matrix 
A(r ) is invertible, f can be obtained from u>b as 

r = A _1 (r)w 6 = A~ 1 (r)J ul (9)9. 


Putting these together, 


r 


A 1 (r) 

0 


UJ b 

X 


0 

Rsb 


V b 


(5.23) 


i.e., the analytic Jacobian J a is related to the body Jacobian J b by 


JaiP) 


O 

X" 

1 

' JUO) ' 


1 

o 

0 Rsb(0) 

_ MO) 


0 Rsb(0) 


MO). 


(5.24) 


5.1.5 Inverse Velocity Kinematics 

The sections above answer the question “What twist results from a given set 
of joint velocities?” The answer, written independently of the frame in which 
twists are represented, is given by 

V = J{9)8. 

Often we are interested in the inverse question: given a desired twist V, what 
joint velocities 6 are needed? This is a question of inverse velocity kinematics, 
which is discussed in more detail in Chapter 6.3. In brief, if J(0) is square (the 
number of joints n is equal to six, the number of elements of a twist) and full 
rank, then 9 = J~ 1 {9)V. If n ^ 6 or the robot is at a singularity, however, 
then J(9) is not invertible. In the case n < 6, arbitrary twists V cannot be 
achieved -the robot does not have enough joints. If n > 6, then we call the 
robot redundant. In this case, a desired twist V places six constraints on the 
joint rates, and the remaining n — 6 freedoms correspond to internal motions of 
the robot that are not evident in the motion of the end-effector. As an example, 
if you consider your arm, from your shoulder to your palm, as a seven-joint open 
chain, when you place your palm at a fixed configuration in space (e.g., on the 
surface of a table), you still have one internal degree of freedom, corresponding 
to the position of your elbow. 


5.2 Statics of Open Chains 

Using our familiar principle of conservation of power, 

power at the joints = (power to move the robot) + (power at end-effector), 

and considering the robot to be at static equilibrium (no power used to move the 
robot), we can equate the power at the joints to the power at the end-effector 3 , 

r T 8 = 

3 We are considering the limiting case as 6 goes to zero, consistent with our assumption 
that the robot is at equilibrium. 
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where r is the set of joint torques. Using the identity V b = J b (9)9, we get 

r = J?(9)F b 

relating the joint torques to the wrench written in the end-effector frame. Sim¬ 
ilarly, 

t=J*{9)F s 

in the fixed space frame. Leaving out the choice of the frame, we can simply 
write 

r = J T (9)F. (5.25) 

Thus if we are given a desired end-effector wrench F and the joint angles 9 , 
and we want the robot to resist an externally-applied wrench —F in order to 
stay at equilibrium, we can calculate the joint torques r needed to generate the 
opposing wrench J 7 . 4 This is important in force control of a robot, for example. 

One could also ask the opposite question, namely, what is the end-effector 
wrench generated by a given set of joint torques? If J T is a 6 x 6 invertible 
matrix, then clearly F = J~ T (9)t. However, if the number of joints n is not 
equal to six, then J T is not invertible, and the question is not well posed. 

If the robot is redundant (n > 6), then even if the end-effector is embedded in 
concrete, the robot is not immobilized and the joint torques may cause internal 
motions of the links. The static equilibrium assumption is no longer satisfied, 
and we need dynamics to know what will happen to the robot. 

If n < 6 and J T £ K rax6 has rank n, then embedding the end-effector in 
concrete will immobilize the robot. If n < 6, however, no matter what r we 
choose, the robot does not actively generate forces in the 6 —n wrench directions 
defined by the null space of J T , 

Null( J t {9)) = {F | J t {9)F = 0}, 

since no actuators act in these directions. The robot can, however, resist ar¬ 
bitrary externally-applied wrenches in the space Null(J T (0)) without moving, 
due to the lack of joints that would allow motions due to these forces. As an 
example, consider a motorized rotating door with a single revolute joint (n = 1) 
and an end-effector frame at the doorknob. The door can only actively gener¬ 
ate a force at the knob that is tangent to the allowed circle of motion of the 
knob (defining a single direction in the wrench space), but it can resist arbitrary 
wrenches in the orthogonal five-dimensional wrench space without moving. 

5.3 Singularity Analysis 

The Jacobian allows us to identify postures at which the robot’s end-effector 
loses the ability to move instantaneously in one or more directions. Such a 

4 If the robot has to support itself against gravity to maintain static equilibrium, these 
torques r must be added to the torques that offset gravity. 
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posture is called a kinematic singularity, or simply a singularity. Math¬ 
ematically a singular posture is one in which the Jacobian J(9) fails to be of 
maximal rank. To understand why, consider the body Jacobian Jb(9), whose 
columns are denoted V b i, i = 1,..., n. Then 


V b 


[ V 6 i(0) V b2 (d) ■■■ V bn {9) 


V b l{9)9i + • • • + Vf m (9)9 n . 


01 


Thus, the tip frame can achieve twists that are linear combinations of the V bl - 
As long as n > 6, the maximum rank that J b {9) can attain is six. Singular 
postures correspond to those values of 9 at which the rank of J b {6) drops below 
the maximum possible value; at such postures the tip frame loses the ability 
to generate instantaneous spatial velocities in in one or more dimensions. The 
loss of mobility at a singularity is accompanied by the ability to resist arbitrary 
wrenches in the direction corresponding to the lost mobility. 

The mathematical definition of a kinematic singularity is independent of the 
choice of body or space Jacobian. To see why, recall the relationship between 
J s (9) and J b (9): J s {9) = Ad Tab ( J b (9 )) = [Ad Ts J Jb(9 ), or more explicitly, 


J s {9) 


Rsb 0 

[Psb] Rsb Rsb 


Jb(9). 


We now claim that the matrix [AdT s6 ] is always invertible. This can be estab¬ 
lished by examining the linear equation 

Rsb 0 
[ Psb ] Rsb Rsb 

Its unique solution is x = y = 0, implying that the matrix [AdT ab ] is invertible. 
Since multiplying any matrix by an invertible matrix does not change its rank, 
it follows that 

rank J s (9) = rank J b {9), 

as claimed; singularities of the space and body Jacobian are one and the same. 



Kinematic singularities are also independent of the choice of the fixed frame 
and the end-effector frame. Choosing a different fixed frame is equivalent to 
simply relocating the robot arm, which should have absolutely no effect on 
whether a particular posture is singular or not. This obvious fact can be verified 
by referring to Figure 5.9(a). The forward kinematics with respect to the original 
fixed frame is denoted T(9 ), while the forward kinematics with respect to the 
relocated fixed frame is denoted T'{9) = PT{6 ), where P € SE( 3) is constant. 
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(b) 


Figure 5.9: Kinematic singularities are invariant with respect to choice of fixed 
and end-effector frames, (a) Choosing a different fixed frame, which is equivalent 
to relocating the base of the robot arm; (b) Choosing a different end-effector 
frame. 


Then the body Jacobian of T'(0), denoted J b {6 ), is obtained from T' 1 T' . A 
simple calculation reveals that 

T'~ lr t' = (T~ 1 P~ 1 )(PT) = T _1 T, 

i.e., J' b (9) = Jb(0), so that the singularities of the original and relocated robot 
arms are the same. 

To see that singularities are independent of the end-effector frame, refer to 
Figure 5.9(b), and suppose the forward kinematics for the original end-effector 
frame is given by T(6), while the forward kinematics for the relocated end- 
effector frame is T'(6) = T{0)Q , where Q £ SE( 3) is constant. This time looking 
at the space Jacobian—recall that singularities of J b {6) coincide with those of 
J s (0) —let J' a {6) denote the space Jacobian of T'{6). A simple calculation reveals 
that 

f'T'- 1 = (TQ)(Q~ 1 T~ 1 ) = TT -1 , 

i.e., J' s {6 ) = J s (9) , so that kinematic singularities are invariant with respect to 
choice of end-effector frame. 

In the remainder of this section we consider some common kinematic singu¬ 
larities that occur in six-dof open chains with revolute and prismatic joints. We 
now know that either the space or body Jacobian can be used for our analysis; 
we use the space Jacobian in the examples below. 

Case I: Two Collinear Revolute Joint Axes 

The first case we consider is one in which two revolute joint axes are collinear 
(see Figure 5.10(a)). Without loss of generality these joint axes can be labeled 
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Figure 5.10: (a) A kinematic singularity in which two joint axes are collinear; 
(b) A kinematic singularity in which three revolute joint axes are parallel and 
coplanar. 


1 and 2. The corresponding columns of the Jacobian are 


V«i(0) 


w s i 

-u>si x qi 


v s 2 (e) 


U S 2 

—U) S 2 X 92 


Since the two joint axes are collinear, we must have oj s \ = ±w S 2 ; let us assume 
the positive sign. Also, uj S i x (qi — q 2 ) = 0 for i = 1,2. Then V s i = V S 2 , 
which implies that V s i and V s2 lie on the same line in six-dimensional space. 
Therefore, the set {V s i, V S 2 ,..., V S 6 } cannot be linearly independent, and the 
rank of J s (9) must be less than six. 


Case II: Three Coplanar and Parallel Revolute Joint Axes 

The second case we consider is one in which three revolute joint axes are par¬ 
allel, and also lie on the same plane (three coplanar axes—see Figure 5.10(b)). 
Without loss of generality we label these as joint axes 1, 2, and 3. In this case 
we choose the fixed frame as shown in the figure; then 

J (9) — Wsl Wsl 

0 — w s i x q 2 —uj s i x (73 

and since q 2 and q 3 are points on the same unit axis, it is not difficult to verify 
that the above three vectors cannot be linearly independent. 


Case III: Four Revolute Joint Axes Intersecting at a Common Point 

Here we consider the case where four revolute joint axes intersect at a common 
point (Figure 5.11). Again, without loss of generality label these axes from 1 
to 4. In this case we choose the fixed frame origin to be the common point of 
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Figure 5.11: A kinematic singularity in which four revolute joint axes intersect 
at a common point. 


intersection, so that qi = ... = = 0. In this case 


Js(0) 


iO s i W s 2 U) s 3 UJ S 4 

0 0 0 0 


The first four columns clearly cannot be linearly independent; one can be writ¬ 
ten as a linear combination of the other three. Such a singularity occurs, for 
example, when the wrist center of an elbow-type robot arm is directly above 
the shoulder. 


Case IV: Four Coplanar Revolute Joints 

Here we consider the case in which four revolute joint axes are coplanar. Again, 
without loss of generality label these axes from 1 to 4. Choose a fixed frame 
such that the joint axes all lie on the x-y plane; in this case the unit vector 
uj S i £ R 3 in the direction of joint axis i is of the form 


^si 


six 
(Xsiy 

0 


Similarly, any reference point qi £ M 3 lying on joint axis i is of the form 


ft = 


qix 

Qiy ) 

0 


and subsequently 


'Csi — ^si X qi — 


0 

0 

^siyqix ^sixqiy 
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The first four columns of the space Jacobian J s {9) are 


^slx Ws2x Ws3x 

^sly ^s2y ^s3y 

0 0 0 

0 0 0 

0 0 0 

^slyQlx ^slxQly ^s2yQ2x ^s2x^2y ^s3yQ3x &s3xQ3y 

which clearly cannot be linearly independent. 


^sAx 

^sAy 

0 

0 

0 

^sAyQAx sAxQAy 


Case V: Six Revolute Joints Intersecting a Common Line 

The final case we consider is six revolute joint axes intersecting a common line. 
Choose a fixed frame such that the common line lies along the z-axis, and select 
the intersection between this common line and joint axis i as the reference point 
qt £ R 3 for axis i\ each qt is thus of the form qi = (0, 0, qi Z ), and 

Vsi ~ tU si X qi = {yJsiyqizi ^sixQiz) 0) 

i = 1,..., 6. The space Jacobian J s (8) thus becomes 


lx 

^s2x 

^s3x 

^sAx 

Msbx 

^s6x 

Wsly 

^s2y 

Ms3y 

U) s Ay 

(jJs5y 

Ws6y 

Wslz 

Ws2z 

Us3z 

WsAz 

Ws5z 

Ws6z 

MslyQlz 

Ws2yq2z 

I^s3y<l3z 

^sAyQAz 

^s5yQ5z 

Ms6yQ6z 

^slxQlz 

~‘^s2xQ2z 

—W a 3 x q3z 

MsAxQAz 

^s5xQ5z 

~^s6xQ6. 

0 

0 

0 

0 

0 

0 


which is clearly singular. 


5.4 Manipulability 

In the previous section we saw that at a kinematic singularity, a robot’s end- 
effector loses the ability to move or rotate in one or more directions. A kinematic 
singularity is a binary proposition—a particular configuration is either kinemat¬ 
ically singular, or it is not—and it is reasonable to ask whether it is possible to 
quantify the proximity of a particular configuration to a singularity. The answer 
is yes; in fact, one can even do better and quantify not only the proximity to a 
singularity, but also determine the directions in which the end-effector’s ability 
to move is diminished, and to what extent. The manipulability ellipsoid 
allows one to geometrically visualize the directions in which the end-effector 
moves with least effort and with greatest effort. 

Manipulability ellipsoids are illustrated for a 2R planar arm in Figure 5.3. 
The Jacobian is given by Equation (5.1). 

For a general n-joint open chain and a task space with coordinates q £ 
R m , where m < n, the manipulability ellipsoid corresponds to the end-effector 
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Figure 5.12: An ellipsoid visualization of q T A~ l q = 1 in the q space R 3 , where 
the principal semi-axis lengths are the square roots of the eigenvalues Aj of A 
and the directions of the principal semi-axes are the eigenvectors Uj. 


velocities for joint rates 6 satisfying ||0|| = 1, a unit sphere in the n-dimensional 
joint velocity space. 5 Assuming J is invertible, the unit joint velocity condition 
can be written 

i = e T e 
= 

= q T J~ T J~ X q 

= q T ( JJ T )~ 1 q = q T A~^q. (5.26) 

If J is full rank (rank m), the matrix A = JJ T £ R mxm is square, symmetric, 
and positive definite, as is A~ 1 . 

Consulting a textbook on linear algebra, we see that for any symmetric 
positive-definite A -1 £ R mxm , the set of vectors q £ R m satisfying 

^A^q= 1 


defines an ellipsoid in the m-dimensional space. Letting m and A, be the eigen¬ 
vectors and eigenvalues of A, the directions of the principal axes of the ellipsoid 
are v. t and the lengths of the principal semi-axes are V~Xi, as illustrated in Fig¬ 
ure 5.12. Furthermore, the volume V of the ellipsoid is proportional to the 
product of the semi-axis lengths, 

V is proportional to \J A 1 A 2 ... A m = \J det(A) = \j det( JJ T ). 

For the geometric Jacobian J (either J}, in the end-effector frame or J s in 
the fixed frame), we can express the 6xn Jacobian as 


m 


Ju(P) 

Jv(0) 


S A two-dimensional ellipsoid is often referred to as an “ellipse,” and an ellipsoid in more 
than three dimensions is often referred to as a “hyperellipsoid,” but here we use the term 
“ellipsoid” independent of the dimension. Similarly, we refer to a “sphere” independent of 
the dimension, instead of using “circle” for two dimensions and “hypersphere” for more than 
three dimensions. 
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where J u is the top three rows of J and J v is the bottom three rows of J. It 
makes sense to separate the two, because the units of angular velocity and lin¬ 
ear velocity are different. This leads to two three-dimensional manipulability 
ellipsoids, one for angular velocities and one for linear velocities. The manipu¬ 
lability ellipsoids are given by principal semi-axes aligned with the eigenvectors 
of A with lengths given by the square roots of the eigenvalues, where A = 
for the angular velocity manipulability ellipsoid and A = J v Jy for the linear 
velocity manipulability ellipsoid. 

When calculating the linear velocity manipulability ellipsoid, it generally 
makes more sense to use the body Jacobian J ^ instead of the space Jacobian J s , 
since we are usually interested in the linear velocity of a point at the origin of 
the end-effector frame, rather than the linear velocity of a point at the origin of 
the fixed space frame. 

Apart from the geometry of the manipulability ellipsoid, it can be useful to 
assign a single scalar measure defining how easily the robot can move at a given 
posture, or how close it is to a singularity. One measure is the ratio between 
the longest and shortest semi-axes of the manipulability ellipsoid, 


Vi(A) 


\A max (A) 

\J Amin ( 7^) 


' A ma x(A) 
Amin (A) 


> 1 , 


where A = JJ T . When pi(A) is low, close to one, then the manipulability 
ellipsoid is nearly spherical or isotropic, meaning that it is equally easy to 
move in any direction. This is generally a desirable situation. As the robot 
approaches a singularity, pi(A) goes to infinity. 

A similar measure p 2 (A) is just the square of pi (A), also known as the 
condition number of the matrix A = JJ T , 


A 


max (A) 




Again, smaller values, close to one, are preferred. The condition number of a 
matrix is commonly used to characterize the sensitivity of the result of a matrix 
multiplication by a vector to small errors in the vector. 

A final measure is simply proportional to the volume of the manipulability 
ellipsoid, 

^{A) = \J AiA 2 . •. = \J det(A). 

In this case, unlike the first two measures, a larger value is better. 

Just like the manipulability ellipsoid, a force ellipsoid can be drawn for joint 
torques t satisfying ||r|| = 1. Beginning from r = J T {d)^, we arrive at similar 
results as above, except now the ellipsoid satisfies 


1 = f T JJ T f = 


where B = (JJ T ) -1 = A -1 . For the force ellipsoid, the matrix B plays the 
same role as A in the manipulability ellipsoid, and it is the eigenvectors and the 
square roots of eigenvalues of B that define the shape of the force ellipsoid. 
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Since eigenvectors of any invertible matrix A are also eigenvectors of B = 
A -1 , the principal axes of the force ellipsoid are aligned with the principal axes 
of the manipulability ellipsoid. Furthermore, since the eigenvalues of B = A _1 
associated with each principal axis are the reciprocals of the corresponding eigen¬ 
values of A , the lengths of the principal semi-axes of the force ellipsoid are given 
by 1/y/Xi, where A i are the eigenvalues of A. Thus the force ellipsoid is obtained 
from the manipulability ellipsoid simply by stretching the manipulability ellip¬ 
soid along each principal axis j by a factor of 1/Aj. Furthermore, since the 
volume Va of the manipulability ellipsoid is proportional to the product of the 
semi-axes, \Ai ^2 • • ■, and the volume Vb of the force ellipsoid is proportional to 
1 /\/AiA 2 ..., the product of the two volumes VaVb is constant independent of 
the joint variables 6. Therefore, positioning the robot to increase the manipula¬ 
bility ellipsoid volume measure fj ,3 (A) simultaneously reduces the force ellipsoid 
volume measure This also explains the observation in the beginning of 

the chapter that, as the robot approaches a singularity, Va goes to zero while 
Vb goes to infinity. 

5.5 Summary 

• Let the forward kinematics of an n-link open chain be expressed in the 
following product of exponentials form: 

T(9) = e [Sl]01 ---e [Sn]0n M. 

The space Jacobian J s {9) £ R 6xra relates the joint rate vector 6 £ R” 
to the end-effector spatial twist V s via V s = J s {9)9. The ith column of 
J s (0) is 

Vsiid') -^-d e [5 1 ]e 1 

for i = 2,..., n, with the first column V s i(0) = <5>i. V S i is the screw vector 
for joint i expressed in space frame coordinates, with the joint values 9 
assumed to be arbitrary rather than zero. 

• Let the forward kinematics of an n-link open chain be expressed in the 
following product of exponentials form: 

T{9) = Me [SllSl • • • e 1 * 3 " 1 ® 71 . 

The body Jacobian Jb{9) £ M 6xn relates the joint rate vector 9 £ 1" to 
the end-effector body twist V& = (cub, Vb) via Vb = Jb(9)9. The ith column 
of Jb(0) is 

Vbi{9) = Ad e _ [s „ ]fl „ ... e —[B i+ 1 ]9 i+1 

for i = n — 1,..., 1, with V{, n (0) = B n . Vbi is the screw vector for joint i 
expressed in body frame coordinates, with the joint values 9 assumed to 
be arbitrary rather than zero. 
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• The body Jacobian is related to the space Jacobian via the relation 

J s (0) = [Ad T JJ b (0) 

J b {9) = [Ad T J J a (0) 

where T sb = T{6). 

• Consider a spatial open chain with n one-dof joints that is also assumed 
to be in static equilibrium. Let r G R" denote the vector of joint torques 
and forces, and T G R 6 be the wrench applied at the end-effector, in either 
space or body frame coordinates. Then r and J- are related by 

r = J b T (0)Ji = J s r (0)J- S . 

• A kinematically singular configuration for an open chain, or more simply 
a kinematic singularity, is any configuration 9 G R" at which the rank 
of the Jacobian is not maximal. For six-dof spatial open chains consisting 
of revolute and prismatic joints, some common singularities include (i) 
two collinear revolute joint axes; (ii) three coplanar and parallel revolute 
joint axes; (iii) four revolute joint axes intersecting at a common point; 
(iv) four coplanar revolute joints, and (v) six revolute joints intersecting 
a common line. 

• The manipulability ellipsoid describes how easily the robot can move in 
different directions. For a Jacobian J , the principal axes of the manip¬ 
ulability ellipsoid are defined by the eigenvectors of JJ T , and the corre¬ 
sponding lengths of the principal semi-axes are the square roots of the 
eigenvalues. 

• The force ellipsoid describes how easily the robot can generate forces in 
different directions. For a Jacobian J, the principal axes of the force 
ellipsoid are defined by the eigenvectors of (JJ T ) _1 , and the corresponding 
lengths of the principal semi-axes are the square roots of the eigenvalues. 

• Measures of the manipulability and force ellipsoid include the ratio of the 
longest principal semi-axis to the shortest; the square of this measure; and 
the volume of the ellipsoid. The first two measures indicate that the robot 
is far from a singularity if they are small (close to one). 

5.6 Software 

Jb = JacobianBody(Blist,thetalist) 

Computes the body Jacobian J b (0) € R 6xrl given a list of joint screws B, ex¬ 
pressed in the body frame and a list of joint angles. 

Js = JacobianSpace(Blist,thetalist) 

Computes the space Jacobian J s (9) G R 6x?I given a list of joint screws Si ex¬ 
pressed in the fixed space frame and a list of joint angles. 
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5.7 Notes and References 

The terms spatial velocity and spatial force were first coined by Roy Feath- 
erstone [32], and are also referred to in the literature as twists and wrenches, 
respectively. There is a well developed calculus of twists and wrenches that 
is covered in treatments of classical screw theory, e.g., [13], [4], Singularities 
of closed chains are discussed in the later chapter on closed chain kinematics. 
Manipulability ellipsoids and their dual, force ellipsoids, are discussed in greater 
detail in [97]. 
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5.8 Exercises 



1 . A wheel of unit radius is rolling to the right at a rate of 1 rad/sec (see 
Figure 5.13; the dotted circle shows the wheel at t = 0). 

(a) Find V s (f), the space velocity of the {b} frame, as a function of t. 

(b) Find the linear velocity of the {b} frame origin expressed in {s} frame 
coordinates. 

2 . The 3R planar open chain of Figure 5.14(a) is shown in its zero position. 

(a) Suppose the tip must apply a force of 5 N in the x-direction. What torques 
should be applied at each of the joints? 

(b) Suppose the tip must now apply a force of 5 N in the y-direction. What 
torques should be applied at each of the joints? 


3. Answer the following questions for the 4R planar open chain of Figure 5.14(b). 
(a) Derive the forward kinematics in the form 

T{0 ) = e i s 2]82 e [S 3 ]6 3 e [S4]e 4 



(a) (b) 


Figure 5.14: (a) A 3R planar open chain, (b) A 4R planar open chain. 
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where each Si € R 3 and M £ SE( 2). 

(b) Derive the body Jacobian. 

(c) Suppose the chain is in static equilibrium at the configuration 61=62 = 
0,03 = |,04 = — f, and a force / = (10,10,0) and moment m = (0,0,10) are 
applied to the tip (both / and m are expressed with respect to the fixed frame). 
What are the torques experienced at each of the joints? 

(d) Under the same conditions as (c), suppose that a force / = (—10,10,0) 
and moment m = (0,0,—10) are applied to the tip. What are the torques 
experienced at each of the joints? 

(e) Find all kinematic singularities for this chain. 



Figure 5.15: Two fingers grasping a can. 


4 . Figure 5.15 shows two fingers grasping a can. Assume that all contacts are 
point contacts with sufficiently large friction coefficient. Frame {b} is attached 
to the center of the can. Frames {bi} and {b 2 } are attached to the can at the two 
contact points as shown, fi = (/i )X , f^ y , / 1>2 ) T is the force applied by fingertip 
1 to the can, expressed in {bi} coordinates. Similarly, /2 = (f 2 ,x, f 2 ,y, f 2 ,z) T is 
the force applied by fingertip 2 to the can, expressed in {b 2 } coordinates. 

(a) Assume the system is in static equilibrium, and find the total spatial force 
Tb applied by the two fingers to the can; express your result in {b} coordinates. 

(b) Suppose .Text an arbitrary external spatial force applied to the can {T ext is 
also expressed in frame {b} coordinates). Find all J-gxt that cannot be resisted 
by the fingertip forces. 

5 . Referring to Figure 5.16, the rigid body rotates about the point (L,L) with 
angular velocity 8 = 1. 

(a) Find the position of point P on the moving body with respect to the fixed 
reference frame in terms of 6 . 

(b) Find the velocity of point P in terms of the fixed frame. 

(c) What is T s b, the displacement of frame {s} as seen from the fixed frame {s}? 

(d) Find the twist of T s t in body coordinates. 

(e) Find the twist of T s b in space coordinates. 
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(f) What is the relation between the twists from (d) and (e)? 

(g) What is the relation between the twist from (d) and P from (b)? 

(h) What is the relation between the twist from (e) and P from (b)? 



6. Figure 5.17 shows a design for a new amusement park ride. The rider sits at 
the location indicated by the moving frame {b}. The fixed frame {s} is attached 
to the top shaft as shown. R = 1 m and L = 2m, and the two joints 0± and 02 
rotate at a constant angular velocity of 1 rad/sec. 

(a) Suppose t = 0 at the instant shown in the figure. Find the linear velocity 
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Figure 5.18: RRP robot shown in its zero position. 


Vb and angular velocity u>b of the rider as a function of time t. Express your 
answer in moving frame {b} coordinates. 

(b) Find the linear velocity of the rider as a function of time t. Express your 
answer in fixed frame {s} coordinates. 

7 . The RRP robot in Figure 5.18 is shown in its zero position. 

(a) Write the screw axes in the space frame. Evaluate the forward kinematics 
when 9 = (90°, 90°, 1). Hand-draw or use a computer to show the arm and the 
end-effector frame in this configuration. Provide the space Jacobian J s at this 
configuration. 

(b) Write the screw axes in the end-effector body frame. Evaluate the forward 
kinematics when 6 = (90°,90°,1) and confirm that you get the same result. 
Provide the body Jacobian Jb at this configuration. 



Figure 5.19: RPR robot. 


8. The RPR robot of Figure 5.19 is shown in its zero position. The fixed and 
end-effector frames are respectively denoted {s} and {b}. 

(a) Find the space Jacobian J s {9) for arbitrary configurations 9 € M 3 . 

(b) Assume the manipulator is in its zero position. Suppose an external force 
f £ R 3 is applied to the {b} frame origin. Find all directions of / that can be 
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L , L 



Figure 5.20: A spatial 3R open chain. 


resisted by the manipulator with zero torques. 

9 . Find all kinematic singularities of the 3R wrist with the forward kinematics 

ft — g[“l]®lg[*2]02g[w3]03 

where wi = ( 0 , 0 , 1 ) T , w 2 = (l/v^ 2 , 0 , l/-\/ 2 ) T , and W 3 = ( 1 , 0 , 0 ) T . 

10 . In this exercise we derive the analytic Jacobian for an n-link open chain 
corresponding to the exponential coordinates on SO( 3). 

(a) Given an n x n matrix A(t) parametrized by t that is also differentiable with 
respect to t, its exponential X(t) = e A is then an n x n matrix that is always 
nonsingular. Prove the following: 

X~ l X = [ e~ A{t)s A{t)e ms ds 

Jo 

XX- 1 = [ e A{t)s A(t)e- A( - t)s ds. 

Jo 

(b) Use the above result to show that for r(t) € M 3 and R(t) = the 

angular velocity in the body frame, [w&] = R T R is related to f by 

UJ b = 

A(r) = 

(c) Derive the corresponding formula relating the angular velocity in the space 
frame, [w s ] = RR T , with r. 

11 . The spatial 3R open chain of Figure 5.20 is shown in its zero position. 

(a) In its zero position, suppose we wish to make the end-effector move with 


A{r)r 

I 1 — cos | 


IMI 2 


-M + 


r - sm r 


llrll 3 


M 2 . 
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Figure 5.21: An RRRP spatial open chain. 


-b---+ 



Figure 5.22: Singularities of a 6R open chain. 


linear velocity uti p = ( 10 , 0 , 0 ), where v t ; p is expressed with respect to the space 
frame {s}. What are the required input joint velocities 84,82,83? 

(b) Suppose the robot is in the configuration 8 \ = 0 , 6*2 = 45°, 83 = —45°. 
Assuming static equilibrium, suppose we wish to generate an end-effector force 
fb = ( 10 , 0 , 0 ), where /{, is expressed with respect to the end-effector frame {b}. 
What are the required input joint torques T \, T 2 , T 3 ? 

(c) Under the same conditions as in (b), suppose we now seek to generate an 
end-effector moment = ( 10 , 0 , 0 ), where mb is expressed with respect to the 
end-effector frame {b}. What are the required input joint torques ti,T 2 ,T 3 ? 

(d) Suppose the maximum allowable torques for each joint motor are 

INI <10, || t 2 || <20, INI <5. 

In the zero position, what is the maximum force that can be applied by the tip 
in the end-effector frame x-direction? 

12 . The RRRP chain of Figure 5.21 is shown in its zero position. 

(a) Determine the body Jacobian Jb{ 9 ) when 81 = 6*2 = 0, 6*3 = 7 t/ 2 , 84 = L. 

(b) Determine the linear velocity of the end-effector frame in fixed frame coor¬ 
dinates when 6*1 = 6*2 = 0, 6*3 = 7 t/ 2 , 84 = L and 9\ = 82 = 83 = 84 = 1. 


13 . For the 6R spatial open chain of Figure 5.22, 
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Figure 5.23: A kinematic singularity involving prismatic and revolute joints. 



Figure 5.24: A spatial PRRRRP open chain. 


(a) Determine its space Jacobian J s (9). 

(b) Find its kinematic singularities. Explain each singularity in terms of the 
alignment of the joint screws, and the directions in which the end-effector loses 
one or more degrees of freedom of motion. 

14 . Show that a six-dof spatial open chain is in a kinematic singularity when any 
two of its revolute joint axes are parallel, and any prismatic joint axis is normal 
to the plane spanned by the two parallel revolute joint axes (see Figure 5.23). 

15 . The spatial PRRRRP open chain of Figure 5.24 is shown in its zero position. 

(a) At the zero position, find the first three columns of the space Jacobian. 

(b) Find all configurations at which the first three columns of the space Jacobian 
become linearly dependent. 

(c) Suppose the chain is in the configuration 9\ = 62 = O 3 = $5 = 6 q = 
0,64 = 90°. Assuming static equilibrium, suppose a pure force fb = (10,0,10) 
is applied to the origin of the end-effector frame, where fb is expressed in terms 
of the end-effector frame. Find the joint torques ti,T 2,73 experienced at the 
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L 


Figure 5.25: A PRRRRR spatial open chain. 


first three joints. 

16 . Consider the PRRRRR spatial open chain of Figure 5.25 shown in its zero 
position. The distance from the origin of the fixed frame to the origin of the 
end-effector frame at the home position is L. 

(a) Determine the first three columns of the space Jacobian J s . 

(b) Determine the last two columns of the body Jacobian 

(c) For what value of L is the home position a singularity? 

(d) In the zero position, what joint torques must be applied in order to generate 
a pure end-effector force of 100 N in the —z direction? 



17 . The PRRRRP robot of Figure 5.26 is shown in its zero position. 

(a) Find the first three columns of the space Jacobian J s (9) 

(b) Assuming the robot is in its zero position and 9 = (1, 0, 1, -1, 2, 0) T , 
the spatial velocity V s of the end-effector frame. 


find 
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(c) Is the zero position a kinematic singularity? Explain your answer. 



Figure 5.27: An RRPRPR open chain shown at its zero position. 


18 . The six-dof RRPRPR open chain of Figure 5.27 has fixed frame {s} and 
end-effector frame {b} attached as shown. At its zero position, joint axes 1, 2 
and 6 lie on the y-z plane of the fixed frame, and joint axis 4 is aligned along 
the fixed frame x-axis. 

(a) Find the first three columns of the space Jacobian J s (0). 

(b) At the zero position, let 6 = (1, 0, 1, -1, 2, 0) T . Find the spatial velocity V s 
of the end-effector frame at the instant shown. 

(c) Is the zero position a kinematic singularity? Explain your answer. 

19 . The spatial PRRRRP open chain of Figure 5.28 is shown in its zero position. 

(a) Determine the first four columns of the space Jacobian J s {0). 

(b) Determine whether the zero position is a kinematic singularity. 

(c) Calculate the joint torques required for the tip to apply the following end- 
effector wrenches: 



Figure 5.28: A spatial PRRRRP open chain with a skewed joint axis. 
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Figure 5.29: A spatial RRPRRR open chain. 


(i) J- S = (0,1,-1,1,0,0) T 

(ii) T s = ( 1 , — 1 , 0 , 1 , 0 , — 1 ) T . 


20 . The spatial RRPRRR open chain of Figure 5.29 is shown in its zero position. 

(a) For the fixed frame {0} and tool frame {t} as shown, express the forward 
kinematics in the product of exponentials form 

T(0) = e [ s, 2] e 2 e [ s 3\Q3 e \Si}di e [Ss\es e {Se,\8e fyj- 

(b) Find the first three columns of the space Jacobian J s (0). 

(c) Suppose that the fixed frame {0} is moved to another location {O'} as shown 
in the figure. Find the first three columns of the space Jacobian J s (6) with 
respect to this new fixed frame. 

(d) Determine if the zero position is a kinematic singularity, and if so, provide 
a geometric description in terms of the joint screw axes. 

21 . The construction robot of Figure 5.30 is used for applications that require 
large, high impact forces. Kinematically the robot can be modelled as a RPRRR 
open chain. 0 2 is a prismatic joint, and 9 4 is defined to be the angle between 
links 3 and 4 as shown in the figure. The robot is shown in its zero position 
in Figures 5.30(a) and 5.30(b); the origin of the tool frame {t} is located at 
coordinates ( '^ iL ^+ L ^ , Lx-VHl? j n terms of the fixed {s}-frame coordinates. 

(a) The forward kinematics T st can be expressed as 

T at = e [Si\Si e [S 2 ] 62 e [S 3 \S3 e [S4\S4]y[_ 

Find and S 4 . 

(b) In reality, joints (0i, 9 2 , $3, 85) are actuated. At the zero position, suppose 
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Figure 5.30: A construction robot. 


the joint velocities are set to (0i,02,0 3 , O 5 = (1,0,1, 2). Find the spatial velocity 
V s of the tool frame. 

(c) Now suppose joints 0\ and 62 are fixed at zero, and only joints 63 and 
65 are actuated. Suppose L± = L 2 , and find the relation between 63 and 63 at 
which the manipulability ellipsoid becomes a circle. 

(d) Under the same conditions as (c), but now with 63 = 7t/ 6 and d 5 = 0, 
in what direction of ( f Xl fy ) can the tip exert a maximum force? Assume the 
maximum exertable torques at joint 3 and 5 are the same. 


22 . Figure 5.31 shows an RRPRRR arm exercise robot used for stroke patient 
rehabilitation 6 . 

(a) Assume the manipulator is in its zero position, and suppose that Mq c € 
SE( 3) is the displacement from frame {0} to frame {c}, and M ct 6 SE( 3) is 
the displacement from frame {c} to frame {t}. Express the forward kinematics 
Tot in the form 

T ot = e^^e^l^Moce^l^e^l^Mcte^ 5 ] 05 ^- 46 ! 06 . 


Find A 2 , A 1 ,.As- 

(b) Suppose that d 2 = 90° and all the other joint variables are fixed at zero. Set 
the joint velocities to ( 61 , d 2 , d 3 , 6 ^ 63 , d 6 ) = (1,0,1, 0, 0,1), and find the spatial 
velocity 14 of the tool frame in frame {0} coordinates. 

®Nef, Tobias, Marco Guidali, and Robert Riener, ’’ARMin III arm therapy exoskeleton 
with an ergonomic shoulder actuation,” Applied Bionics and Biomechanics 6(2): 127-142, 
2009 
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Figure 5.31: Rehabilitation robot ARMin III. 


(c) Is the configuration described in part (b) a kinematic singularity? Explain 
your answer. 

(d) Suppose a person now operates the rehabilitation robot. At the configuration 
described in part (b), a spatial force -A e ]| )OW is applied to the elbow link, and 
a spatial force -F^ip is applied to the last link. Both .Fgikow an d -^tip are 
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expressed in frame { 0 } coordinates and are given by 

•^elbow = [ 1 0 0 0 0 1 ]' 

^tip =[01011 Of. 

Find the joint torques that must be applied in order for the robot to maintain 
static equilibrium. 

23 . Consider an n-link open chain, with reference frames attached to each link. 
Let 

T ok = e^ e '---e^ 0 *M k , k = l,...,n 

be the forward kinematics up to link frame {k}. Let J s (9) be the space Jacobian 
for T 0n . The columns of J s ( 6 ) are denoted 

J s {0) = [ V sl (6») V s „(0)]. 

Let [Vfc] = TTq)} be the spatial velocity of link frame {k} in frame {k} coordi¬ 
nates. 

(a) Derive explicit expressions for V 2 and V 3 . 

(b) Based on your results from (a), derive a recursive formula for 14+i in terms 
of Vfc, Vgi,..., V s ,fc+i, and 6 . 

24 . Write a program that allows the user to enter the link lengths L\ and L 2 of 
a 2R planar robot (Figure 5.32), and a list of configurations of the robot (each 
as the joint angles ( 61 , 62 )), and plots the manipulability ellipse at each of those 
configurations. The program should plot the arm at each configuration (two line 
segments) and the manipulability ellipse centered at the endpoint of the arm. 
Choose the same scaling for all the ellipses so that they are easily visualized 
(e.g., the ellipse should usually be shorter than the arm, but not so small that 
you can’t easily see it). The program should also print the three manipulability 
measures fj,\, H 2 , H 3 for each configuration. 

(i) Choose Li = L 2 = 1 and plot the arm and its manipulability ellipse at 
the four configurations (—10°, 20°), (60°, 60°), (135°, 90°), (190°, 160°). At 
which of these configurations does the arm appear most isotropic? Does 
this agree with the manipulability measures calculated by the program? 

(ii) Does the eccentricity of the ellipse depend on 6 {l On 0 2 ? Explain your 
answers. 

(iii) Choose Li = L 2 = 1. Hand-draw the arm at (—45°,90°). Hand-draw 
the endpoint linear velocity vector arising from 61 = 1 rad/s and d 2 = 0 . 
Hand-draw the endpoint linear velocity vector arising from 6 1 = 0 and 
62 = 1 rad/s. Hand-draw the vector sum of these two to get the endpoint 
linear velocity when 6 \ = 1 rad/s and 62 = 1 rad/s. 
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Figure 5.32: Left: The 2R robot arm. Right: The arm at four configurations. 



25 . Modify the program in the previous exercise to plot the force ellipse. 
Demonstrate it at the same four configurations as in the first part of the previous 
exercise. 

26 . The kinematics of the 6R UR5 robot are given in Chapter 4.1.2. 

(i) Give the numerical space Jacobian J s when all joint angles are ir/2. Divide 
the Jacobian matrix into an angular velocity portion J u (joint rates act 
on angular velocity) and a linear velocity portion J v (joint rates act on 
linear velocity). 

(ii) At this configuration, calculate the directions and lengths of the prin¬ 
cipal semi-axes of the three-dimensional angular velocity manipulability 
ellipsoid (based on JJ) and the directions and lengths of the principal 
semi-axes of the three-dimensional linear velocity manipulability ellipsoid 
(based on J v ). 

(iii) At this configuration, calculate the directions and lengths of the principal 
semi-axes of the three-dimensional moment (torque) force ellipsoid (based 
on JJ) and the directions and lengths of the principal semi-axes of the 
three-dimensional linear force ellipsoid (based on J v ). 


27 . The kinematics of the 7R WAM robot are given in Chapter 4.1.3. 

(i) Give the numerical body Jacobian when all joint angles are tt/2. Divide 
the Jacobian matrix into an angular velocity portion (joint rates act 
on angular velocity) and a linear velocity portion J v (joint rates act on 
linear velocity). 

(ii) At this configuration, calculate the directions and lengths of the prin¬ 
cipal semi-axes of the three-dimensional angular velocity manipulability 
ellipsoid (based on JJ) and the directions and lengths of the principal 
semi-axes of the three-dimensional linear velocity manipulability ellipsoid 
(based on J v ). 
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(iii) At this configuration, calculate the directions and lengths of the principal 
semi-axes of the three-dimensional moment (torque) force ellipsoid (based 
on J u ) and the directions and lengths of the principal semi-axes of the 
three-dimensional linear force ellipsoid (based on J v ). 


28 . Examine the software functions for this chapter in your favorite program¬ 
ming language. Verify that they work the way you expect. Can you make them 
more computationally efficient? 
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Chapter 6 

Inverse Kinematics 


For a general n degree-of-freedom open chain with forward kinematics T( 6 ), 
6 £ R n , the inverse kinematics problem can be stated as follows: given a ho¬ 
mogeneous transform X £ SE( 3), find solutions 6 that satisfy T{ 6 ) = X. To 
highlight the main features of the inverse kinematics problem, let us consider 
the two-link planar open chain of Figure 6.1(a) as a motivational example. Con¬ 
sidering only the position of the end-effector and ignoring its orientation, the 
forward kinematics can be expressed as 


X 


L 1 cos 6 \ + L 2 cos(6*i + d 2 ) 

. y . 


Li sin 61 + L 2 sin(0i + d 2 ) 


Assuming L\ > L 2l the set of reachable points, or the workspace, is an annulus 
of inner radius L\ — L 2 and outer radius L\ + L 2 . Given some end-effector 
position ( x,y ), it is not hard to see that there will be either zero, one, or 
two solutions depending on whether (x, y) lies in the exterior, boundary, or 
interior of this annulus, respectively. When there are two solutions, the angle 
at the second joint (the “elbow” joint) may be positive or negative. These two 
solutions are sometimes called “lefty” and “righty” solutions, or “elbow-up” and 
“elbow-down” solutions. 

Finding an explicit solution (0i : 9 2 ) for a given ( x,y ) is also not difficult. 
For this purpose, we will find it useful to introduce the two-argument arctan¬ 
gent function atan2(y,x), which returns the angle from the origin to a point 
(x,y) in the plane. It is similar to the inverse tangent tan ~ 1 (y/x), but whereas 
tan -1 only returns angles in the range (— 7 t/ 2 , 7 t/ 2 ), since points (x,y) are in¬ 
distinguishable from points (—£, — y), the atan2 function returns angles in the 
range (—7r,7r]. For this reason, atan2 is sometimes called the four-quadrant 
arctangent. 

We also recall the law of cosines, 

c 2 = a 2 + b 2 — 2 ab cos C, 

where a, 6, and c are the lengths of the three sides of a triangle and C is the 
interior angle of the triangle opposite the side of length c. 
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(a) Workspace and lefty and righty 
configurations. 



(b) Geometric solution. 


Figure 6.1: 2 R planar open chain inverse kinematics. 


Referring to Figure 6.1(b), angle /?, restricted to lie in the interval [0, 7 r], can 
be determined from the law of cosines, 

L\ + — 2 LiL 2 cos /3 = x 2 + y 2 , 

from which it follows that 

/r2 i r2_.2 „,2' 

-i / L i + ^2 x — y 


(3 = cos 

Also from the law of cosines 


2LiL 2 


a = cos 


_r ( x 2 + y 2 + L\ - L\ 
2L 1 ^/x 2 + y 2 


The angle 7 is determined using the two-argument arctangent function, 7 = 
atan2 (y,x). With these, the righty solution to the inverse kinematics is 


h = 7 — a, d 2 = 7r — /3 


and the lefty solution is 


Si = 7 + a, O 2 = (3 — 7T. 

If x 2 + y 2 lies outside the range [L\ — L 2 , Li + L 2 ], then no solution exists. 

This simple motivational example illustrates that for open chains, the in¬ 
verse kinematics problem may have multiple solutions; this is in contrast to the 
forward kinematics, where a unique end-effector displacement T exists for given 
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Figure 6.2: Inverse position kinematics of a 6R PUMA-type arm. 


joint values 6. In fact, three-link planar open chains have an infinite number 
of solutions for points (a :,y) lying in the interior of the workspace; in this case 
the chain possesses an extra degree of freedom, and is said to be kinematically 
redundant. 

In this chapter we first consider the inverse kinematics of spatial open chains 
with six degrees of freedom. At most a finite number of solutions exists in this 
case, and we consider two popular structures—the PUMA and Stanford robot 
arms- -for which analytic inverse kinematic solutions can be easily obtained. 
For more general open chains, we adapt the Newton-Raphson method for non¬ 
linear root finding to the inverse kinematics problem. The result is an iterative 
numerical algorithm that, provided an initial guess of the joint variables is suffi¬ 
ciently close to a true solution, eventually converges to a solution. The chapter 
concludes with a discussion of pseudoinverse-based inverse kinematics solutions 
for redundant open chains. 

6.1 Analytic Inverse Kinematics 

We begin by writing the forward kinematics of a spatial six-dof open chain in 
the following product of exponentials form: 

T(0) = e^- Sl ^ Sl e^- s ^ Sa e^ S3 ^ 3 e^ Ss ^ B e^ S6 ^ ee M. 


Given some end-effector frame X £ SE( 3), in the inverse kinematics problem 
we seek solutions 9 £ R 6 that satisfy T(9) = X. In the following subsections we 
derive analytic inverse kinematic sollutions for the PUMA and Stanford arms. 

6.1.1 6R PUMA-Type Arm 

We first consider a 6 R arm of the PUMA type. Referring to Figure 6.2, when 
the arm is placed in its zero position, (i) the two shoulder joint axes intersect 
orthogonally at a common point, with joint axis 1 aligned in the z 0 direction and 
joint axis 2 aligned in the xp direction; (ii) joint axis 3 (the elbow joint) lies in 
the xo-y 0 plane and is aligned parallel with joint axis 2; (iii)joint axes 4, 5, and 6 
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(a) Elbow arm with (b) Kinematic diagram, 

offset. 


Figure 6.3: A 6R PUMA-type arm with a shoulder offset. 


Z H 



Figure 6.4: Singular configuration of the zero-offset 6 R PUMA-type arm. 

(the wrist joints) intersect orthogonally at a common point (the wrist center) to 
form an orthogonal wrist; for the purposes of this example we assume joint axes 
4, 5, and 6 are aligned in the zo, y 0 , and xo directions, respectively. The arm 
may also have an offset at the shoulder (see Figure 6.3). The inverse kinematics 
problem for PUMA-type arms can be decoupled into an inverse position and 
inverse orientation subproblem, as we now show. 

We first consider the simple case of a zero-offset PUMA-type arm. Referring 
to Figure 6.2 and expressing all vectors in terms of fixed frame coordinates, 
denote components of the wrist center p £ R 3 by p = ( Px,Py,Pz )• Projecting p 
onto the x-y plane, it can be seen that 

9i = tan -1 ) 

where the atan2 function can be used instead of tan -1 . Note that a second valid 
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Figure 6.5: Four possible inverse kinematics solutions for the 6 R PUMA type 
arm with shoulder offset. 


solution for 9i is given by 


9i = tan 1 J + 7 r, 

provided that the original solution for 9 2 is replaced by 7 x — 9 2 . As long as 
p x ,Py 7 ^ 0, both these solutions are valid. When p x = p y = 0 the arm is in a 
singular configuration (see Figure 6.4), and there are infinitely many possible 
solutions for 9\. 

If there is an offset d\ ^ 0 as shown in Figure 6.3, then in general there will be 
two solutions for 9\, denoted the right and left arm solutions (Figure 6.3). As 

/ r 2_d2 

seen from the figure, 9 i = </>—a, where <f> = tan -1 (^-) and a = tan” 1 (M- s -—I) = 
tan -1 ( v x d y —-). The second solution is given by 


= tan 1 ( — ) — tan 1 

Px 


-\]Pi + P 2 V ~ 


Determining angles 9 2 and 9 3 for the PUMA-type arm now reduces to the inverse 
kinematics problem for a planar two-link chain: 


cos 03 = 


s 2 — a| — a 2 


20203 

Pi + P 2 V + ipl - di ) 2 - a\ ~ 
2a 2 a,3 

























196 


Inverse Kinematics 


If we let cos 0 3 = D , then 0 3 is given by 



62 can be obtained in a similar fashion as 


02 = tan 1 


tan 1 



The two solutions for 0 3 correspond to the well-known elbow-up and elbow- 
down configurations for the two-link planar arm. In general, a PUMA-type arm 
with an offset will have four solutions to the inverse position problem, as shown 
in Figure 6.5; the upper postures are called left-arm solutions (elbow-up and 
elbow-down), while the lower postures are called right-arm solutions (elbow-up 
and elbow-down). 

We now solve the inverse orientation problem, i.e., finding ( 04 , 0 5 , 0 6 ) given 
the end-effector frame orientation. This is completely straightforward: having 
found ( 0 i, 02 , 0 3 ), the forward kinematics can be manipulated into the form 


e [St]84 e [S s ] 6 Be [S e \ 6 e _ e -[<S 3 ]03 e -[S 2 ]02 e -[Si]0i 


where the right-hand side is now known, and the w* components of S 4 , S 3 , Sq 
are 


co 4 = ( 0 , 0 , 1 ) 

W 5 = (0,1,0) 

w 6 = (1,0,0). 


Denoting the 50(3) component of the right-hand side of Equation (6.2) by R, 
the wrist joint angles ( 64 , 0 5 , 9q) can be determined as the solution to 

Rot(z, 6 b)Rot(y, 0s)Rot(x, Q§) = R , 

which corresponds exactly to the ZYX Euler angles as derived in Appendix B. 

6.1.2 Stanford-Type Arms 

If the elbow joint in a 6 R PUMA-type arm is replaced by a prismatic joint 
as shown in Figure 6 . 6 , we then have a Stanford-type arm. Here we consider 
the inverse position kinematics for the arm of Figure 6 . 6 ; the inverse orientation 
kinematics is identical to that for the PUMA-type arm and is not repeated here. 
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Figure 6.6: The first three joints of a Stanford-type arm. 


The first joint variable d 3 an be found in a similar fashion to the PUMA-type 
arm: = tan -1 (^) (provided that p x and p y are not both zero). 0 2 is then 

found from Figure 6.6 to be 

d 2 = tan” 1 

where r 2 = p 2 x + p 2 and s = p z — d\. Similar to the PUMA-type arm, a second 
solution for 0 1; 0 2 is given by 

9\ = 7T + tan -1 ^ — ^ 

0 2 = 7T — tan -1 ^ 

The translation distance d 3 is found from the relation 

(d 3 + a 2 ) 2 =r 2 + s 2 
as 

d 3 = \/r 2 + s 2 

= \JpI+pI + (Pz - di) 2 - a 2 

Ignoring the negative square root solution for d 3 , we obtain two solutions to 
the inverse position kinematics as long as the wrist center p does not intersect 
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the x-axis of the fixed frame. If there is an offset, then as in the case of the 
PUMA-type arm there will be a left and right arm solution. 

If the elbow joint in the generalized 6R PUMA-type arm is replaced by a 
prismatic joint, the resulting arm is then referred to as a generalized Stanford- 
type arm. The inverse kinematics proceeds in the same way as for the general¬ 
ized PUMA-type arm; the only difference occurs in the first step (obtaining 63 ). 
The screw vector for the third joint now becomes 1 S 3 = ( 0 ,^ 3 ), with 11 173 11 = 1, 
and 03 is found by solving the equation 

|| e [5 3 ]e3p|| = C) 

for some given p£l 3 and nonnegative positive scalar c. The above equation 
reduces to solving the following quadratic in 0 3 : 

^3 + 2 (p T U 3)03 + (||p|| 2 — c 2 ) = 0. 

Imaginary, as well as negative solutions, naturally should be excluded. 

6.2 Numerical Inverse Kinematics 

Iterative numerical methods can be applied if the inverse kinematics equations 
do not admit analytic solutions. Even in cases where an analytic solution does 
exist, numerical methods are often used to improve the accuracy of these solu¬ 
tions. For example, in a PUMA-type arm, the last three axes may not exactly 
intersect at a common point, and the shoulder joint axes may not be exactly or¬ 
thogonal. In such cases, rather than throw away any analytic inverse kinematic 
solutions that are available, such solutions can be used as the initial guess in an 
iterative numerical procedure for solving the inverse kinematics. 

There exist a variety of iterative methods for finding the roots of a nonlinear 
equation, and our aim is not to discuss these in detail -any text on numerical 
analysis will cover these methods in depth —but rather to develop ways in which 
to transform the inverse kinematics equations such that they are amenable to 
existing numerical methods. However, it is useful to review one fundamental 
approach to nonlinear root-finding, the Newton-Raphson method. Also, in situ¬ 
ations where an exact solution may not exist and we seek the closest approximate 
solution, or conversely, an infinity of inverse kinematics solutions exists (i.e., if 
the robot is kinematically redundant) and we seek a solution that is optimal 
with respect to some criterion, methods of optimization are needed. We be¬ 
gin this section with a discussion of the Newton-Raphson method for nonlinear 
root-finding, and also the first-order necessary conditions for optimization. 

6.2.1 Newton-Raphson Method 

To numerically solve the equation f{6) = 0 for a given differentiable function 
/ : R. —> R, assume 0 q is an initial guess for the solution, and expand f{6) to 
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first-order about Oq: 

df 

f{0) = f(0 o ) + - do) + higher-order terms. 

Keeping only the terms up to first-order, set f(0) =0 and solve for 8 to obtain 


9 = 

Using this value of 8 as the new guess for the solution and repeating the above, 
we get the following iteration: 

9k+i = e k -(^(8 k )\ 8 k . 


The above iteration is repeated until some stopping criterion is satisified, e.g., 
| f(9k) — /(0fc+i)|/|/(0fc)| < e for some user-prescribed threshold value e. 

The same formula applies for the case when / is multi-dimensional, i.e., 
/ : R" —> l n , in which case 


df_ 

dd 


( 0 ) 


9fi 

oe 1 


(d) 



sfcW 


G M 


nxn 


The case when the above matrix fails to be invertible is discussed further in the 
later section describing the numerical inverse kinematics algorithm. 


6.2.2 Optimization Basics 

Suppose x* G R. is a local minimum of a twice-differentiable objective function 
f(x), f : R —> R, in the sense that for all x near x*, we have f(x) > f{x*). We 
can then expect that the slope of /( x) at x* is zero, i.e., 


dl 

dx 


(**) = o, 


and also that 




If / is nrulti-dimensional, i.e., / : —> M, and all partial derivatives of / 

exist up to second-order, then a necessary condition for x* G K" to be a local 
minimum is that its gradient be zero: 


V/(**) = 


dJ2(x*) 

d Xl > 




= 0 . 


As an example, consider the linear equation Ax = b 1 where A G R mxrl and 
b G R m (m > n) are given. Because there are more constraints (m) than 
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variables (n), in general a solution to Ax = b will not exist. Suppose we seek 
the x that best approximates a solution, in the sense of minimizing 

mm f(x) = 7 ,l|bhc — 6|| 2 = ^(Acc — b) T (Ax — b) = ^ x T A T Ax — 2 b T Ax + b T b. 

The first-order necessary condition is given by 

A t Ax - A T b = 0. (6.3) 

If rank(A) = n, then A T A £ R" xn is invertible, and the solution to (6.3) is 

x* = ( A T A)~ 1 A r b. 

Now suppose we wish to find, among all x £ R" that satisfy g(x) = 0 for 
some differentiable g : R™ —> R m (typically to < n to ensure that there exists an 
infinity of solutions to g{x) = 0 ), the x* that minimizes the objective function 
f(x). Suppose x* is a local minimum of / that is also a regular point of the 
surface parametrized implicitly by g{x) = 0 , i.e., x* satisfies g{x*) = 0 and 

rank ^( x*) = to. 
ox 

Then from the fundamental theorem of linear algebra, it can be shown that 
there exists some A* £ R m (called the Lagrange multiplier) that satisfies 

V/(x*) + ^ (x*)A*=0 (6.4) 

ox 

Equation (6.4) together with g{x*) = 0 constitute the first-order necessary con¬ 
ditions for x* to be a feasible local mininum of f{x). Note that these two 
equations represent n + to equations in the n + m unknowns x and A. 

As an example, consider the following quadratic objective function f(x): 

min /( x) = -x t Qx + c T x 

xe* n 2 

subject to the linear constraint Ax = 6 , where Q £ R” is symmetric positive- 
definite (that is, x T Qx > 0 for all x £ 1R”), and the matrix A £ R mxn , to < n, 
is of maximal rank to. The first-order necessary conditions for this equality- 
constrained optimization problem are 

Qx + A T A = —c 
Ax = b. 

Since A is of maximal rank and Q is invertible, the solutions to the first-order 
necessary conditions can be obtained after some manipulation as 

x = Gb+(I-GA)Q~ 1 c 
A = Bb + BAQ~ 1 c 
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where G £ R” xm an d B £ M 1 ™*" 7, are j e fi nec j as 

G = Q~ 1 A t B, B = (AQ- 1 A t )- 1 . 

Both the proof of the existence of Lagrange multipliers and the derivation of 
the solution to the first-order necessary conditions are further explored in the 
exercises at the end of this chapter. 

6.2.3 Numerical Inverse Kinematics Algorithm 

Suppose we express the end-effector frame using a coordinate vector x governed 
by the forward kinematics x = f(6), a nonlinear vector equation mapping the 
n joint coordinates to the m end-effector coordinates. Assume / : K" —> is 

differentiable, and let Xd be the desired end-effector coordinates. The goal is to 
find joint coordinates 6 d such that 


Xd ~ f(9 d ) = 0. 


Given an initial guess do which is “close by” a solution 0^, the kinematics can 
be expressed as the Taylor expansion 


x d = f(0 d ) = f(0 o ) 



{ 0 d ~ d 0 ) + h.o.t., 
Ae 


J(e 0 ) 


(6.5) 


where J(d o) £ M mxrl is the coordinate Jacobian evaluated at do- Truncating 
the Taylor expansion at first-order, we can approximate Equation (6.5) as 

J(d 0 )Ad = x d - f(do). (6.6) 

Assuming J(0o) is square (to = n) and invertible, we can solve for Ad as 

Ad = J~ 1 (do)(x d -f(do)). (6.7) 

If the kinematics are linear, i.e., the higher-order terms in Equation (6.5) are 
zero, then the new guess d\ = do + Ad would exactly satisfy Xd = f(d i). If not, 
the new guess d\ should be closer to the root than 0 O , and the process is then 
repeated, with the sequence {0o, 0i, 0 2 , • ■ •} converging to dd (Figure 6.7). 

As indicated in Figure 6.7, if there are multiple inverse kinematics solutions, 
the iterative process tends to converge to the solution that is “closest” to the 
initial guess do- You can think of each solution as having its own basin of 
attraction. If the initial guess is not in one of these basins (e.g., the initial guess 
is not sufficiently close to a solution), the iterative process may not converge. 

In practice, for computational efficiency reasons, Equation ( 6 . 6 ) is often 
solved without directly calculating the inverse J~ 1 (do). More efficient tech¬ 
niques exist for solving a set of linear equations Ax = b for x. For example, for 
invertible square matrices A, the LU decomposition of A can be used to solve 
for x with fewer operations. In MATLAB, for example, the syntax 
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Figure 6.7: The first step of the Newton-Raphson method for nonlinear root¬ 
finding for a scalar x and 6 . In the first step, the slope —df/dd is evaluated 
at the point ( 9 0 ,Xd — f{0 o)). In the second step, the slope would be evaluated 
at the point (0\,Xd — f( 6 1 )), and eventually the process would converge to 6^. 
Note that an initial guess to the left of the plateau of Xd — f(6) would likely 
result in convergence to the other root of Xd — /(#), and an initial guess at or 
near the plateau would result in a large initial |A0| and the iterative process 
may not converge at all. 


x = A\b 

solves for x without computing A -1 . 

If J is not invertible, either because it is not square or because it is singular, 
then J~ l in Equation (6.7) does not exist. Equation (6.6) can still be solved 
(or approximately solved) for Ad by replacing J -1 in Equation (6.7) with the 
Moore-Penrose pseudoinverse J'. For any equation of the form Jy = z , where 
J £ K mX11 , y £ R", and z £ K m , the solution 

y* = J*z 


falls into one of two categories: 

• The solution y* exactly satisfies Jy* = z, and for any solution y exactly 
satisfying Jy = z, ||y*|| < [|y||. In other words, among all solutions, the 
solution y* minimizes the two-norm. There can be an infinite number of 
solutions y to Jy = z if the robot has more joints n than end-effector 
coordinates m, i.e., the Jacobian J is “fat.” 

• If there is no y that exactly satisfies Jy = z, then y* minimizes the two- 
norm of the error, i.e., || Jy* — z|| < || Jy — z|| for any y £ R n . This 
case corresponds to rank(J) < m, i.e., the robot has fewer joints n than 
end-effector coordinates m (a “tall” Jacobian J) or is at a singularity. 

Many programming languages provide functions to calculate the pseudoin¬ 
verse; for example, the usage in MATLAB is 
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y = pinv(J)*z 

In the case that J is full rank (rank m for n > m or rank n for n < to), i.e., the 
robot is not at a singularity, the pseudoinverse can be calculated as 

J' = J T (JJ T )~ 1 if J is fat, n > m (called a right inverse , since JJ* = I) 

J' = (J T J)^ 1 J T if J is tall, n < to (called a left inverse , since J^J = I). 

Replacing the Jacobian inverse with the Jacobian pseudoinverse, Equation (6.7) 
becomes 

Ae = j\6 0 )(x d -f(6 0 )). (6.8) 

If rank(J) < to, then the solution Ad calculated in Equation ( 6 . 8 ) may not 
exactly satisfy Equation ( 6 . 6 ), but it satisfies this condition as closely as possible 
in a least-squares sense. If n > to, then the solution is the smallest joint variable 
change (in the two-norm sense) that exactly satisfies Equation ( 6 . 6 ). 

Equation ( 6 . 8 ) suggests the Newton-Raphson iterative algorithm for finding 

e d : 

(i) Initialization: Given x d £ M m and an initial guess 9q £ R ra . Set i = 0. 

(ii) Set e = x d — /($*). While ||e|| > e for some small e: 

• Set 0i +1 = 9i + J^(0i)e. 

• Increment i. 

To modify this algorithm to work with a desired end-effector configuration 
represented as T sd £ SE( 3) instead of as a coordinate vector x d , we can replace 
the coordinate Jacobian J with the end-effector body Jacobian J b £ R 6xn . 
Note, however, that the vector e = x d — f[0i), representing the direction from 
the current guess (evaluated through the forward kinematics) to the desired 
end-effector configuration, cannot simply be replaced by T s( ] — T s f,{0j)\ the pseu¬ 
doinverse of Jb should act on a body twist 14 £ R 6 . To find the right analogy, 
we should think of e = x d — f(9i) as a velocity vector which, if followed for 
unit time, would cause a motion from f(9i) to x d . Similarly, we should look 
for a body twist 14 which, if followed for unit time, would cause a motion from 
T s b(9i) to the desired configuration T sd . 

To find this 14, we first calculate the desired configuration in the body frame, 

T bd {0i) = T^(9i)T ad = T bs {9i)T sd . 

Then 14 is determined using the matrix logarithm, 


[V 6 ] = log TUOi)- 


This leads to the following inverse kinematics algorithm, analogous to the 
coordinate vector algorithm: 

(i) Initialization: Given T sd and an initial guess 9q £ R". Set i = 0. 
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Figure 6.8: (Left) A 2R robot. (Right) The goal is to find the joint angles 
yielding the end-effector frame {goal}, corresponding to 9\ = 30°, 02 = 90°. The 
initial guess is (0°,30°). After one Newton-Raphson iteration, the calculated 
joint angles are (34.23°, 79.18°). The screw axis that takes the initial frame to 
the goal frame (the dotted line) is also indicated. 


(ii) Set [14] = log (T sb 1 (9 i )T sd ). While ||w fe || > or ||n 6 || > e v for small e w ,e„: 

• Set di+i = 9i + Jl(9i)Vb- 

• Increment i. 

An equivalent form can be derived in the space frame, using the space Ja¬ 
cobian J s (9) and the spatial twist V s = [AdT s J14. 

For this numerical inverse kinematics method to converge, the initial guess 
9q should be sufficiently close to a solution 9 d . This condition can be satisfied 
by starting the robot from an initial home configuration where both the actual 
end-effector configuration and the joint angles are known, and ensuring that 
the requested end-effector position T sd change slowly relative to the frequency 
of the calculation of the inverse kinematics. Then, for the rest of the robot’s 
run, the calculated 9d at the previous timestep serves as the initial guess 9q for 
the new T s d at the next timestep. 


Example: Planar 2R Robot 


The body Jacobian Newton-Raphson inverse kinematics algorithm is applied to 
the 2R robot in Figure 6.8. Each link is 1 m in length, and we would like to find 
the joint angles that place the tip of the robot at (x,y) = (0.366 m, 1.366 m), 
which corresponds to 


Ted = 


-0.5 -0.866 0 0.366 

0.866 -0.5 0 1.366 

0 0 10 

0 0 0 1 
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as shown by the frame {goal} in Figure 6 . 8 . The forward kinematics, expressed 
in the end-effector frame, are given by 







' 0 ' 


' 0 ' 

1 

0 

0 

2 ' 


0 


0 

0 

1 

0 

0 

II 

1 

, b 2 = 

1 

0 

0 

1 

0 

o 

0 

0 

0 

0 

1 


2 


1 






_ 0 _ 


0 


Our initial guess to the solution is 9g = (0, 30°), and we specify an error tolerance 
of Cu: = 0.001 rad (or 0.057°) and e v = 10 -4 m (100 microns). The progress 
of the Newton-Raphson method is illustrated in the table below, where only 
the (u> z b,v x b,v y b) components of the body twist 14 are given, since the robot’s 
motion is restricted to the x-y plane: 


i 

9i (in degrees) 

(x, y) 

Vfr — (id z b 5 Vxb ? Vyb) 

Nil 

Ikll 

0 

(0.00,30.00°) 

(1.866,0.500) 

(1.571,0.498,1.858) 

1.571 

1.924 

1 

(34.23°, 79.18°) 

(0.429,1.480) 

(0.115,-0.074,0.108) 

0.115 

0.131 

2 

(29.98°, 90.22°) 

(0.363,1.364) 

(-0.004,0.000,-0.004) 

0.004 

0.004 

3 

(30.00°, 90.00°) 

(0.366,1.366) 

( 0 . 000 , 0 . 000 , 0 . 000 ) 

0.000 

0.000 


The iterative procedure converges to within the tolerances after three it¬ 
erations. Figure 6.8 shows the initial guess, the goal configuration, and the 
configuration after one iteration. Notice that the first v x b calculated is positive, 
even though the origin of the goal frame is in the — iq, direction of the initial 
guess. This is because the constant body velocity 14 that takes the initial guess 
to {goal} in one second is a rotation about the screw axis indicated in the figure. 

6.3 Inverse Velocity Kinematics 

To control a robot to follow a desired end-effector trajectory T s d(t ), one solution 
is to calculate the inverse kinematics Od(kAt) at each discrete time step k , then 
control the joint velocities to be 

6 = ( OdikAt) - 9((k - l)At))/At 

during the time interval [(fc — l)At, kAt], This is a type of feedback controller, 
since the desired new joint angles 9d(kAt) are being compared to the most 
recently measured actual joint angles 9{(k — l)Af) to calculate the commanded 
joint velocities. 

Another option that avoids the computation of inverse kinematics is to cal¬ 
culate the required joint velocities 9 directly from the relationship J9 = 14, 
where the desired end-effector twist 14 and J are expressed with respect to the 
same frame: 


9 = j\9)V d . 


( 6 . 9 ) 
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The desired twist Vd(t) can be chosen to be T sd 1 (t)T s d(t) (the body twist of 
the desired trajectory at time t) or T s d{t)T~ d (t) (the spatial twist), depending 
on whether the body Jacobian or space Jacobian is used, but small velocity 
errors are likely to accumulate over time, resulting in increasing position error. 
Preferably a position feedback controller would be designed to choose Vd{t) to 
keep the end-effector following T s d(t) with little position error. Feedback control 
is discussed in Chapter 11. 

In the case of a redundant robot with n > 6 joints, of the (n — 6 )-dimensional 
set of solutions satisfying Equation (6.9), the use of the pseudoinverse J^{9) 
returns the 9 minimizing the two-norm ||0|| = \J9 T 9. This is appealing; the 
motion is the minimum joint velocity motion that achieves the desired end- 
effector twist. 

We could also choose to give the individual joint velocities different weight¬ 
ing. For example, as we will see later, the kinetic energy of a robot can be 
written 

\e T M(e)e , 

where M{9) is the symmetric, positive-definite, configuration-dependent inertia 
matrix of the robot. We may also seek a path that minimizes some configuration- 
dependent potential energy function h{9) (for example, h(9) could be the gravi¬ 
tational potential energy, or an external elastic potential energy that may arise 
from mechanical springs connected between a robot’s links or at the joints). 
The time derivative of h{9) is then the rate of change of the potential, or power: 

^-h{ 6 ) = \7h(9) T 9. 
at 

The following optimization problem can therefore be formulated: 

min - 9 t M{6)9 , +V/i(< 9) t <9 
e 2 

subject to the constraint J{6)9 = Vd ■ From the first-order necessary conditions 
for optimality, i.e., 

J T A = M9 + Vh 
V d = J9 , 

the optimal 9 and A can be derived as follows: 

9 = GV d + (J - GJ)M~ 1 S7h 
A = BVd + BJM-^h, 

where B £ J£ mxrn anc j Q g jg^xm are ^ e fj nec j as 

B = (JM~ 1 J t )~ 1 
G = M- 1 J t (JM “ 1 J t )~ 1 = M-'jT'B. 
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Recalling the static relation r = J T T from the previous chapter, the Lagrange 
multiplier A can be interpreted as a spatial force in task space. Moreover, from 
the expression A = BVd + BJM~ 1 Xh , the first term BVd can be interpreted as 
a dynamic force generating the end-effector velocity Vd, while the second term 
BJM~ l \/h can be interpreted as a static force required to keep the end-effector 
stationary. 

If a potential function h(q) is not specified and we wish to choose the joint 
velocities 0 to minimize the kinetic energy of the robot (intuitively, minimize 
the velocities of the joints moving a lot of mass), while still satisfying the desired 
end-effector twist, the solution 

9 = M- 1 J^JAT 1 J T )" 1 V d , 

corresponding to the weighted Moore-Penrose pseudoinverse J 1 = M ” 1 J T {JM~ l J T ) 
minimizes the weighted norm 0 T MO. 

6.4 A Note on Closed Loops 

A desired end-effector trajectory over a time-interval [0,f/] is a closed loop if 
T s d( 0) = T s d(tf). It should be noted that numerical methods for calculating 
inverse kinematics for redundant robots, at either the configuration or velocity 
levels, are likely to yield motions that are not closed loops in the joint space, 
i.e., 0(0) ^ 9(tf). If closed-loop motions in joint space are required, an extra 
set of conditions on the inverse kinematics must be satisfied. 


6.5 Summary 

• Given a spatial open chain with forward kinematics T(0), 9 £ R ra , in the 
inverse kinematics problem one seeks to find, for some given X £ SE( 3), 
solutions 8 that satisfy X = T(9). Unlike the forward kinematics, the 
inverse kinematics problem can possess multiple solutions, or no solutions 
in the event that X lies outside the workspace. For a spatial open chain 
with n joints and an X in the workspace, n = 6 typically leads to a finite 
number of inverse kinematic solutions, while n > 6 leads to an infinite 
number of solutions. 

• The inverse kinematics can be solved analytically for the six-dof PUMA- 
type robot arm, a popular 6 R design consisting of a 3 R orthogonal axis 
wrist connected to a 2 R orthogonal axis shoulder by an elbow joint. 

• Another class of open chains that admit analytic inverse kinematic solu¬ 
tions are Stanford-type arms; these arms are obtained by replacing the 
elbow joint in the generalized 6 R PUMA-type arm by a prismatic joint. 
Geometric inverse kinematic algorithms similar to those for PUMA-type 
arms have been developed. 
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• Iterative numerical methods are used in cases where analytic inverse kine¬ 
matic solutions are not available. These typically involve solving the in¬ 
verse kinematics equations through an iterative procedure like the Newton- 
Raphson method, and they require an initial guess at the joint variables. 
The performance of the iterative procedure depends to a large extent on 
the quality of the initial guess, and in the case that there are several pos¬ 
sible inverse kinematic solutions, the method finds the solution that is 
“closest” to the initial guess. Each iteration is of the form 

di +1 = 0 i + J+(fli)V, 

where J\0) is the pseudoinverse of the Jacobian J( 6 ) and V is the twist 
that takes T(0j) to T sd in one second. 

6.6 Software 

[thetalist,success] = IKinBody(Blist,M,T,thetalistO,eomg,ev) 

Uses iterative Newton-Raphson to calculate the inverse kinematics given the 
list of joint screws Bi expressed in the end-effector frame, the end-effector home 
configuration M, the desired end-effector configuration T, an initial guess at the 
joint angles $o, and the tolerances e u and e v on the final error. If a solution is 
not found within a maximum number of iterations, then success is false. 

[thetalist,success] = IKinSpace(Slist,M,T,thetalistO,eomg,ev) 

Similar to IKinBody, except the joint screws Si are expressed in the space frame, 
and the tolerances are interpreted in the space frame. 

6.7 Notes and References 

The inverse kinematics of the most general 6 R open chain is known to have up 
to 16 solutions; this result was first proved by Lee and Liang [63], and also by 
Raghavan and Roth [101]. Iterative numerical procedures for finding all sixteen 
solutions of a general 6 R open chain are reported in [77]. A summary of inverse 
kinematics methods for kinematically redundant robot arms are discussed in 
[123]. The repeatability conditions for kinematically redundant inverse kine¬ 
matics schemes are examined in [116]. 
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Figure 6.10: A 6 R open chain. 


6.8 Exercises 


1. Write a program that solves the analytical inverse kinematics for a planar 3R 
robot with link lengths L i = 3, L 2 = 2, and L\ = 1, given the desired position 
(x, y) and orientation 9 of a frame fixed to the tip of the robot. Each joint has 
no joint limits. Your program should find all of the solutions (how many are 
there in the general case?), give the joint angles for each, and draw the robot 
in these configurations. Test the code for the case of (x, y, 9) = (4, 2, 0). 

2 . Solve the inverse position kinematics (you do not need to solve the orienta¬ 
tion kinematics) of the 6 R open chain robot shown in Figure 6.9. 


3. Find the inverse kinematics solutions when the end-effector frame {T} of 
the 6 R open chain robot shown in Figure 6.10 is set to {T’} as shown. The 
orientation of {T} at the zero position is the same as that of the fixed frame 
{s}, and {T’} is the result of a pure translation of {T} along the y s -axis. 

4. The RRP open chain of Figure 6.11 is shown in its zero position. Joint axes 
1 and 2 intersect at the fixed frame origin, and the end-effector frame origin p 
is located at ( 0 , 1 , 0 ) when the robot is in its zero position. 

(a) Suppose 9 1 = 0. Solve for 9 2 and 9 3 when the end-effector frame origin p is 
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at (—6, 5, v^). 

(b) If joint 1 is not fixed to zero but instead allowed to vary, find all inverse 
kinematic solutions ( 61 , 62 , 63 ) for the same p given in (a). 

5. The four-dof robot of Figure 6.12 is shown in its zero position. Joint 1 
is a screw joint of pitch h. Given the end-effector position p = ( Px,Py,Pz ) 
and orientation R = e^ a , where z = (0,0,1) and a £ [0, 27r], find the inverse 
kinematics solution ($i, 62 , 03 , 64 ) as a function of p and a. 



Figure 6.12: Open chain with screw joint 
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6 . Figure 6.13(a) shows a surgical robot, which can be modelled as an RRPRRP 
open chain as shown in Figure 6.13(b). 

(a) In the general case, how many inverse kinematic solutions will exist for a 
given end-effector frame? 

(b) Suppose coordinates for points A and B in the fixed frame are respectively 
given as (xa, Ua, za) and (xb, Vb, zb)- Solve the inverse kinematics for 6 *i, 02 , 
S 3 , Si, S 5 : find an explicit formula for {Si, 62 , S 3 ), while for ( 04 , 65 ), just describe 
the procedure. 



(a) Surgical robot 



Figure 6.13: Surgical robot and kinematic model 


7. In this exercise you will draw a plot of a scalar Xd~ f{ 8 ) vs. a scalar 0 (similar 
to Figure 6.7) with two roots. Draw it so that for some initial guess 9q, the 
iterative process actually jumps over the closest root and eventually converges 
to the further root. Hand-draw the plot and show the iteration process that 
results in convergence to the further root. Comment on the basins of attraction 
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Figure 6.14: A 3R wrist. 


of the two roots in your plot. 

8 . Use Newton-Raphson iterative numerical root finding to perform two steps 
of finding the root of 



when your initial guess is {x\, yi) = (1,1). Write the general form of the gradient 
(for any guess (x,y)) and compute the results of the first two iterations. You 
can do this by hand or write a program. If you write a program, submit your 
code and the printout of the results. Also, give all of the correct roots, not just 
the one that would be found from your initial guess. How many are there? 

9. Modify the function IKinBody to print out the results of each Newton- 
Raphson iteration, similar to the table for the 2R robot example in Section 6.2. 
Show the table produced when the initial guess for the 2R robot of Figure 6.8 
is (0,30°) and the goal configuration corresponds to (90°, 120°). Turn in your 
code. 

10. The 3 R orthogonal axis wrist mechanism of Figure 6.14 is shown in its zero 
position, with joint axes 1 and 3 collinear. 

(a) Given a desired wrist orientation R € SO(3), derive an iterative numerical 
procedure for solving its inverse kinematics. 

(b) Perform a single iteration of Newton-Raphson root-finding using body-frame 
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Figure 6.15: A 3R nonorthogonal chain. 


numerical inverse kinematics. First write the forward kinematics and Jacobian 
for general configurations of the wrist. Then apply your results for the specific 
case of an initial guess of 9\ = d 3 =0, 0 2 = 7r/6, with a desired end-effector 
frame at 


R = 


i 



\/2 

0 


V2 

0 


0 ■ 
0 
1 


G 50(3). 


If you write code, submit the code together with the results. 


11. The 3R nonorthogonal chain of Figure 6.15 is shown in its zero position. 

(a) Derive a numerical procedure for solving the inverse position kinematics 
numerically; that is, given some end-effector position p as indicated in the figure, 
find (0i,0 2 ,0 3 ). 

(b) Given an end-effector orientation R £ 50(3), find all inverse kinematic 
solutions ( 01 , 02 , 03 )- 

12. Use the function IKinSpace to find joint variables O d of the UR5 (Chap¬ 
ter 4.1.2) satisfying 


T(0 d ) = T sd = 


0 1 0 -0.5 

0 0-1 0.1 
-1 0 0 0.1 

0 0 0 1 


Distances are in meters. Use = 0.001 (0.057 degrees) and e v = 0.0001 
(0.1 mm). For your initial guess 0o, choose all joint angles as 0.1 rad. If the 
configuration is outside the workspace, or if you find that the zero configuration 
is too far from a final answer to converge, you may demonstrate IKinBody 
using another T sd . Note that the numerical inverse kinematics routines do not 
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respect joint limits, so it is possible for your routine to find solutions that are 
not achievable by the actual robot. 


13. Use the function IKinBody to find joint variables 9 d of the WAM (Chap¬ 
ter 4.1.3) satisfying 


T{6 d ) = T sd = 


1 0 0 0.5 

0 10 0 
0 0 1 0.4 

0 0 0 1 


Distances are in meters. Use e u = 0.001 (0.057 degrees) and e v = 0.0001 
(0.1 mm). For your initial guess 9 0 , choose all joint angles as 0.1 rad. If the 
configuration is outside the workspace, or if you find that the zero configuration 
is too far from a final answer to converge, you may demonstrate IKinBody using 
another T sd . 


14. The Fundamental Theorem of Linear Algebra (FTLA) states that given a 
matrix A € R mxn , 

null(A) = range(A T ) _L 
null(A T ) = range(A)- 1 -, 


where null(A) denotes the null space of A (i.e., the subspace of R" of vectors 
that satisfy Ax = 0), range(A) denotes the range or column space of A (i.e., 
the subspace of R m spanned by the columns of A), and range(A)- 1 - denotes 
the orthogonal complement to range(A) (i.e., the set of all vectors in R m that 
are orthogonal to every vector in range(A)). In this problem you are asked 
to use FTLA to prove the existence of Lagrange multipliers for the equality 
constrained optimization problem. Let f(x), f : M" —> JR. differentiable, be 
the objective function to be minimized, x must satisfy the equality constraint 
g(x ) = 0 for given differentiable g : R” —> R m . 

(a) Suppose x* is a local minimum. Let x(t) be any arbitrary curve on the 
surface parametrized implicitly by g(x) = 0 (implying that g{x{t)) = 0 for all t) 
such that x(0) = x*. Further assume that x* is a regular point of the surface. 
Taking the time derivative of both sides of g(x(t)) = 0 at t = 0 then leads to 


dg 

dx 


(x*)i(0) = 0. 


( 6 . 10 ) 


At the same time, because a:(0) = x* is a local minimum, it follows that f{x(t )) 
(viewed as an objective function in t ) has a local minimum at t = 0, implying 


that 




t =0 


df_ 

dx 


(x*)i(0) = 0. 


( 6 . 11 ) 


Since (6.10) and (6.11) must hold for all arbitrary curves x{t) on the surface 
defined by g{x) = 0, use FTLA to prove the existence of a Lagrange multiplier 
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A* £ R m such that the first-order necessary condition 

Vf(x*) + p-(x*) T \*=0. 


holds. 

15. (a) If A -1 exists, show that 


' A 

D ' 

-1 

' A -1 + EG~ 1 F 

-EG- 1 ' 

c 

B 


i 

1 

Q 

G- 1 


where G = B — CA~ l D , E = A~ l D , and F = CA~ X . 

(b) Use the above result to solve the first-order necessary conditions for the 
equality constrained optimization problem 

min -x T Qx + c T x 
2 

subject to Ax = b , where Q £ R" xn is symmetric positive-definite and A £ 
R™x" is of maximal rank m. 
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Chapter 7 

Kinematics of Closed 
Chains 


Any kinematic chain that contains one or more loops is called a closed chain. 
Several examples of closed chains were encountered in Chapter 2, from the 
planar four-bar linkage to spatial mechanisms like the Stewart-Gough platform 
and Delta robot (Figure 7.1). These mechanisms are examples of parallel 
mechanisms; these are closed chains consisting of a fixed and moving platform 
connected by a set of “legs.” The legs themselves are typically open chains, 
but sometimes can also be other closed chains (like the Delta robot depicted in 
the figure). In this chapter we analyze the kinematics of closed chains, paying 
special attention to parallel mechanisms. 

The Stewart-Gough Platform is used widely as both a motion simulator 
and six-axis force-torque sensor. When used as a force-torque sensor, the six 
prismatic joints experience internal linear forces whenever any external force 
is applied to the moving platform; by measuring these internal linear forces 
one can estimate the applied external force. The Delta robot is a three-dof 
mechanism whose moving platform moves such that it always remains parallel 
to the fixed platform. Because the three actuators are all attached to the three 
revolute joints of the fixed platform, the moving parts are relatively light; this 
allows the Delta to achieve very fast motions. 

Closed chains admit a much greater variety of designs than open chains, 
and their kinematic and static analysis is consequently more complicated. This 
can be traced to two defining features of closed chains: (i) not all joints are 
actuated, and (ii) the joint variables must satisfy a number of loop-closure 
constraint equations, which may or may not be independent depending on the 
configuration of the mechanism. The presence of unactuated (or passive) joints, 
together with the fact that the number of actuated joints may deliberately 
exceed the mechanism’s kinematic degrees of freedom—such mechanisms are 
said to be redundantly actuated makes not only the kinematics analysis 
more challenging, but also introduces new types of singularities not present in 
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Figure 7.1: Two popular parallel mechanisms. 


open chains. 

Recall also that for open chains, the kinematic analysis proceeds in a more 
or less straightforward fashion with the formulation of the forward kinematics 
(e.g., via the product of exponentials formalism) followed by that of the inverse 
kinematics. For general closed chains it is usually difficult to obtain an explicit 
set of equations for the forward kinematics in the form X = T(0), where X £ 
SE( 3) is the end-effector frame and 0 £ R" are the joint coordinates. The more 
effective approaches exploit as much as possible any kinematic symmetries and 
other special features of the mechanism. 

In this chapter we begin with a series of case studies involving some well- 
known parallel mechanisms, and eventually build up a repertoire of kinematic 
analysis tools and methodologies for handling more general closed chains. Our 
focus will be on parallel mechanisms that are exactly actuated, i.e., the number 
of actuated degrees of freedom is equal to the number of degrees of freedom of the 
mechanism. Methods for the forward and inverse position kinematics of parallel 
mechanisms are discussed, followed by the characterization and derivation of 
the constraint Jacobian, and the Jacobians of both the inverse and forward 
kinematics. The chapter concludes with a discussion of the different types of 
kinematic singularities that arise in closed chains. 


7.1 Inverse and Forward Kinematics 

One general observation can be made for serial mechanisms vs. parallel mecha¬ 
nisms: For serial chains, forward kinematics is generally straightforward while 
inverse kinematics may be complex (e.g., multiple solutions or no solutions). For 
parallel mechanisms, the inverse kinematics is often relatively straightforward 
(e.g., given the configuration of a platform, it may not be hard to determine 
the joint variables), while the forward kinematics may be quite complex: an 

















7.1. Inverse and Forward Kinematics 


219 



arbitrarily chosen set of joint variables may be infeasible, or it may correspond 
to multiple possible configurations of the platform. 

In this section we begin with two case studies, the 3xRPR planar parallel 
mechanism, and its spatial counterpart, the 3xSPS Stewart-Gough platform. 
The analysis of these two mechanisms draws upon some simplification techniques 
that result in a reduced form of the governing kinematic equations, which in turn 
can be generalized to the analysis of more general parallel mechanisms. 

7.1.1 3xRPR Planar Parallel Mechanism 

The first example we consider is the 3-dof planar 3xRPR parallel mechanism 
shown in Figure 7.2. A fixed frame {s} and body frame {b} are assigned to 
the platform as shown. The three prismatic joints are typically actuated while 
the six revolute joints are passive. Denote the lengths of each of the three legs 
by Si, i = 1,2,3. The forward kinematics problem is to determine, from given 
values of s = (si, S 2 , S 3 ), the body frame’s position and orientation. Conversely, 
the inverse kinematics problem is to determine s from T s b £ SE( 2). 

Let p be the vector from the origin of the {s} frame to the origin of the {b} 
frame. Let (j) denote the angle measured from the x s axis of the {s} frame to 
the X 5 axis of the {b} frame. Further define the vectors a j, b^, dj, i = 1,2,3, as 
shown in the figure. From these definitions, clearly 


dj = p + hi - a i, 


(7.1) 
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for * = 1,2, 3. Let 


Note that the (aj x ,a, 
exception of the (b 
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,j), i = 1,2,3 are all constant, and that with the 
biy), all other vectors are expressed in {s}-frame coordi- 


1x7 u iy 


nates. To express Equation (7.1) in terms of {s}-frame coordinates, b* must be 
expressed in {s}-frame coordinates. This is straightforward: defining 


Rsb = 


cos cf> — sin <{> 
sin cf> cos (j) 


it follows that 


dix 

_ 

Px 

Rsb 

bix 


&ix 

diy 


. Py . 
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for i = 1, 2, 3. Also, since s 2 = d 2 x + d 2 y , we have 

s 2 = {Vx + b ix coscj) - b iy sin</>— a ix ) 2 

+(p y + b ix sin + b iy cos cj> - a iy ) 2 , (7.2) 

for * = 1,2, 3. 

Formulated as above, the inverse kinematics is trivial to compute: given 
values for (p x . p y , <fi), the leg lengths (si,S 2 ,S 3 ) can be directly calculated from 
the above equations (negative values of Si will not be physically realizable in 
most cases and can be ignored). In contrast, the forward kinematics problem of 
determining the body frame’s position and orientation ( p x ,p y ,(l>) from the leg 
lengths (si, 52 , 53 ) is not trivial. The following tangent half-angle substitution 
transforms the three equations in (7.2) into a system of polynomials in the newly 
defined scalar variable t: 


t 


sin (f> 
cos (j) 


. (t> 

tan — 

2 

2 1 

1 + t 2 
l~t 2 
1 + t 2 ' 


After some algebraic manipulation, the system of polynomials (7.2) can even¬ 
tually be reduced to a single sixth-order polynomial in t; this effectively shows 
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(a) 




(b) 


Figure 7.3: (a) The 3xRPR at a singular configuration. From this configuration, 
extending the legs may cause the platform to snap to a counterclockwise rotation 
or to a clockwise rotation, (b) Two solutions to the forward kinematics when 
all prismatic joint extensions are identical. 


that the 3xRPR mechanism may have up to six forward kinematics solutions. 
Showing that all six mathematical solutions are physically realizable requires 
further verification. 

Figure 7.3(a) shows the mechanism at a singular configuration, where each 
leg length is identical and as short as possible. This configuration is a sin¬ 
gularity, because extending the legs from this symmetric configuration causes 
the platform to rotate either clockwise or counterclockwise; we cannot predict 
which. 1 Singularities are covered in greater detail in Section 7.3. Figure 7.3(b) 
shows two solutions to the forward kinematics when all leg lengths are identical. 

7.1.2 Stewart-Gough Platform 

We now examine the inverse and forward kinematics of the 6xSPS Stewart- 
Gough platform of Figure 7.1(a). In this design, the fixed and moving platforms 
are connected by six serial SPS structures, with the spherical joints passive and 
the prismatic joints actuated. The derivation of the kinematic equations closely 
parallels that of the earlier 3xRPR planar mechanism. Let {s} and {b} denote 
the fixed and body frames, respectively, and let di be the vector directed from 
joint A i to joint Bj, i = 1,..., 6. Referring to Figure 7.1(a), we make the 
following definitions: 

pgK 3 = p in {s}-frame coordinates; 
di £ JR 3 = in {s}-frame coordinates; 
bi £ R 3 = bi in {b}-frame coordinates; 
di £ R 3 = di in {s}-frame coordinates; 

R £ SO(3) = orientation of {b} as seen from {s}. 

In order to derive the kinematic constraint equations, note that vectorially, 

di = p + bj - a,, i = 1,.. .,6. 

1 A third possibility is that the extending legs crush the platform! 





222 


Kinematics of Closed Chains 



Writing the above equations explicitly in {s}-frame coordinates, 

di = p + Rbi — a,i, i = 1 ,..., 6. 

Denoting the length of leg i by Sj, we have 

sf = dfdi = {p + Rbi - ai) T (p + Rbi - m), 

for i = 1,..., 6. Note that a, and bi are all known constant vectors. Writing 
the constraint equations in this form, the inverse kinematics becomes straight¬ 
forward: given p and R, the six leg lengths Sj, i = 1,... ,6 can be evaluated 
directly from the above equations (negative values of s» in most cases will not 
be physically realizable and can be ignored). 

The forward kinematics is not as straightforward: given each of the leg 
lengths Si, i = 1,...,6, we must solve for p £ M 3 and R € 50(3). These 
six constraint equations, together with six further constraints imposed by the 
condition R T R = I, constitute a set of twelve equations in twelve unknowns 
(three for p, nine for R). 

7.1.3 General Parallel Mechanisms 

For both the 3xRPR mechanism and Stewart-Gough Platform, we were able 
to exploit certain features of the mechanism that resulted in a reduced set of 
equations; for example, the fact that the legs of the Stewart-Gough Platform 
can be modelled as straight lines considerably simplified the analysis. In this 
section we briefly consider the case when the legs are general open chains. 
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Consider such a parallel mechanism as shown in Figure 7.4; here the fixed 
and moving platforms are connected by three open chains, and the configuration 
of the moving platform is given by T s b- Denote the forward kinematics of the 
three chains by T\(Q), T 2 (<j>), and T, 3 ( 1 /)), respectively, where 9 E R m , <f> E R n , 
and if) € R p . The loop-closure conditions can be written T s j, = T) (9) = T 2 (cf>) = 
T 3 (ip). Eliminating T s b we get 

Ti(8) = T 2 (4>) (7.3) 

T 2 (</>) = T 3 (# (7.4) 

Equations (7.3) and (7.4) each consist of twelve equations (nine for the rotation 
component and three for the position component), six of which are indepen¬ 
dent (from the rotation matrix constraint R T R = /, the nine equations for the 
rotation component can be reduced to a set of three independent equations). 
Thus there are 24 equations, twelve of which are independent, with n + m + p 
unknown variables. The mechanism therefore has d = n + m+ p— 12 degrees 
of freedom. 

In the forward kinematics problem, given values for d of the joint variables 
(9,ef>,ip), Equations (7.3) and (7.4) can be solved for the remaining joint vari¬ 
ables. Generally this is not trivial and multiple solutions are likely. Once the 
joint values for any one of the open chain legs are known, the forward kinematics 
of that leg can then be evaluated to determine the forward kinematics of the 
closed chain. 

In the inverse kinematics problem, given the body frame displacement T s b E 
SE( 3), we set T = Tj = T 2 = T 3 and solve Equations (7.3) and (7.4) for the 
joint variables (Q,<j>,'ip). As suggested by the case studies, for most parallel 
mechanisms there are often features of the mechanism that can be exploited to 
eliminate some of these equations and simplify them to a more computationally 
amenable form. 


7.2 Differential Kinematics 

We now consider the differential kinematics of parallel mechanisms. Unlike 
the case for open chains, in which the objective is to relate the input joint 
velocities to the twist of the end-effector frame, the analysis for closed chains is 
complicated by the fact that not all of the joints are actuated. Only the actuated 
joints can be prescribed input velocities; the velocities of the remaining passive 
joints must then be determined from the kinematic constraint equations. These 
passive joint velocities are usually required to eventually determine the twist of 
the closed chain’s end-effector frame. 

For open chains, the Jacobian of the forward kinematics is central to veloc¬ 
ity and static analysis. For closed chains, in addition to the forward kinematics 
Jacobian, the Jacobian defined by the kinematic constraint equations—we will 
call this the constraint Jacobian —also plays a central role in velocity and 
static analysis. Usually there are features of the mechanism that can be ex¬ 
ploited to simplify and reduce the procedure for obtaining the two Jacobians. 
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We illustrate with a case study of the Stewart-Gough platform, and show that 
the Jacobian of the inverse kinematics can be obtained straightforwardly via 
static analysis. Velocity analysis for more general parallel mechanisms is then 
detailed. 

7.2.1 Stewart-Gough Platform 

Earlier we saw that the inverse kinematics for the Stewart-Gough platform can 
be solved analytically. That is, given the body frame orientation R £ 50(3) 
and position pgR 3 , the leg lengths s £ R 6 can be obtained analytically in the 
functional form s = g(R,p). In principle one could differentiate this equation 
and manipulate it into the form 


s = G{R,p)V s , (7.5) 

where s £ R 6 denotes the leg velocities, V s £ R 6 is the body frame’s spatial 
twist, and G{R,p) £ R 6x6 is the Jacobian of the inverse kinematics. In most 
cases this procedure will require considerable algebraic manipulation. 

Here we take a different approach based on static analysis. Based on the 
same conservation of power principles that were used to determine the static 
relationship r = J T T for open chains, the static relationship for closed chains 
can also be expressed in exactly the same form. We illustrate with an analysis 
of the Steawert-Gough platform. 

In the absence of external forces, the only forces applied to the moving 
platform occur at the spherical joints. In what follows, all vectors are expressed 
in {s}-frame coordinates. Let 

fi — fliTi 

be the three-dimensional linear force applied by leg i, where hi £ R 3 is a unit 
vector indicating the direction of the applied force, and r, £ R is the magnitude 
of the linear force. The moment generated m, generated by fi is 

mi = Ti x fi, 

where r,; £ R 3 denotes the vector from the {s}-frame origin to the point of 
application of the force (the location of spherical joint i in this case). Since 
neither the spherical joint at the moving platform nor the spherical joint at the 
fixed platform can resist any torques about the joints, the force /, must be along 
the line of the leg. Therefore, instead of calculating the moment m* using the 
spherical joint at the moving platform, we can calculate the moment using the 
spherical joint at the fixed platform: 

mi = qi x fi, 

where qi £ R 3 denotes the vector from the fixed-frame origin to the base joint 
of leg i. Since qi is constant, expressing the moment as qi x fi is preferred. 
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Combining fi and into the six-dimensional wrench Ti = ( mi, fi ), the 
resultant wrench T s on the moving platform is given by 


= £* = £ 


i=i 


i=l L 


n x m 

hi 


Ti 





Tl 

-hi x qi 

■ -h 6 x qe 



hi 

h 6 





. T 6 . 


= J7 t t, 


where J s is the spatial Jacobian of the forward kinematics, with its inverse given 
by 


J. 


-i 


—Tii x qi • • • -n 6 x q 6 

hi ■■■ h 6 


7 . 2.2 General Parallel Mechanisms 

Because of its kinematic structure, the Stewart-Gough platform lends itself par¬ 
ticularly well to a static analysis, as each of the six joint forces are directed along 
their respective legs. The Jacobian (or more precisely, the inverse Jacobian) can 
therefore be derived in terms of the screws associated with each line. In this 
section we consider more general parallel mechanisms where the static analysis 
is less straightforward. Using the previous three-legged spatial parallel mech¬ 
anism of Figure 7.4 as an illustrative example, we derive a general procedure 
for determining the forward kinematics Jacobian that can also be generalized 
to parallel mechanisms. 

The mechanism of Figure 7.4 consists of two platforms connected by three 
legs with m, n, and p joints, respectively. To keep things simple, we assume 
m = n = p = 5, so the mechanism has d, = n + m + p — 12 = 15 degrees 
of freedom (generalizing what follows to different types and numbers of legs is 
completely straightforward). For the fixed and body frames indicated in the 
figure, write the forward kinematics for the three chains as follows: 


Ti(0i, 6 * 2 ,. 

• • ,e 5 ) = 

e [Si]0i e [s 2 ]e 2 .. 

■e [S5]l>5 Mi 

? 2 (<£• 

• ■ ! 05 ) = 

e [Pi]<h e [P 2 ]<h .. 

■ ■ e™* 5 M 2 

T 3 {ip 1 ,^ 2 , ■ 

••,^ 5 ) = 

e [Qi]bi e [Q2]b2 . 

.. e [ Q ^M 3 


The kinematic loop constraints can be expressed as 

Ti(9) = T 2 (0) 

Ti{4>) = t 3 (4 .) 

Since these constraints must be satisfied at all times, we can express their time 
derivatives in terms of their spatial twists, i.e., 

i\T - 1 = TiT - 1 

T2T2 1 = T3T3- 1 . 


(7.6) 

(7.7) 


(7.8) 

(7.9) 
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Since TiT^ 1 = [Vi], where V,; is the spatial twist of chain Vs end-effector frame, 
the above identities can also be expressed in terms of the forward kinematics 
Jacobian for each chain: 


Ji(B)8 = J 2 ( 9)9 
M9)9 = J 3 W 1 P, 

which can be rearranged as 


' Me) - j 2 ( 0 ) 

0 

' e ' 

0 - j 2 ( 0 ) 

M1>) . 

9 

. ip . 


(7.10) 

(7.11) 


(7.12) 


Now rearrange the fifteen joints into those that are actuated and those that 
are passive. Assume without loss of generality that the three actuated joints 
are (9i,pi,ipi)- Define the vector of actuated joints q a £ R 3 and passive joints 
q p £ R 12 as 



' 0 1 ' 


' e 2 ' 

Qa = 

pi 
. pi 

5 Qp - 

p5 


and q = ( q a ,Q P ) £ R 15 . Equation (7.12) can now be rearranged into the form 


[ H a (q) H p (q) 


Qa 

Qp 


= 0, 


(7.13) 


or equivalently 

H a q a + H p q p = 0, (7-14) 

where H a £ R 12x3 and H p £ R 12x12 . If H p is invertible, we have 

q p = - H~ l H a q a . (7.15) 

Assuming H p is invertible, once the velocities of the actuated joints are given, 
the velocities of the remaining passive joints can be obtained uniquely via Equa¬ 
tion (7.15). 

It still remains to derive the forward kinematics Jacobian with respect to 
the actuated joints, i.e., to find J a (q) £ R 6x3 satisfying V s = J a {o)Qa , where V s 
is the spatial twist of the end-effector frame. For this purpose we can use the 
forward kinematics for any of the three open chains; for example, in terms of 
chain 1, J\(6)6 = V s , and from Equation (7.15) we can write 


02 

= 92 Qa 

(7.16) 

0 3 

= glia 

(7.17) 

di 

= glia 

(7.18) 

05 

= gl Qa 

(7.19) 
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where each gi(q) € R 3 , i = 2,...,5, can be obtained from Equation (7.15). 
Defining the row vector ef = (1,0,0), the differential forward kinematics for 
chain 1 can now be written 



(7.20) 


Since we are seeking J a (q) in = J a (q)q<n and q£ = (9i,<pi,ipi), from the 
above it now follows that 



J a {q) = Ji(qi,---,q5) 9s{q) T 


(7.21) 


9i{q) T 

gs{q) T 


The above could also have been derived using either chain 2 or chain 3. 

Given values for the actuated joints q a , it still remains to solve for the passive 
joints q p from the loop constraint equations. Eliminating as many elements of 
q p in advance will obviously simplify matters. The second point to note is 
that H p (q) may become singular, in which case q p cannot be obtained from 
q a . Configurations in which H p (q) becomes singular correspond to actuator 
singularities, which are discussed in the next section. 

7.3 Singularities 

Characterizing the singularities of closed chains involves many more subtleties 
than for open chains. In this section we highlight the essential features of closed- 
chain singularities via two planar examples: a four-bar linkage (see Figure 7.5), 
and a five-bar linkage (see Figure 7.6). Based on these examples we classify 
closed chain singularities into three basic types: actuator singularities, con¬ 
figuration space singularities, and end-effector singularities. 

We begin with the four-bar linkage of Figure 7.5. Recall from Chapter 2 that 
its C-space is a one-dimensional curve embedded in a four-dimensional ambient 
space (each dimension is parametrized by one of the four joints). Projecting the 
C-space onto the joint angles (0, </>) leads to the curve shown in Figure 7.5. In 
terms of 9 and </>, the kinematic loop constraint equations for the four-bar can 
be expressed as 



(7.22) 










228 


Kinematics of Closed Chains 


<P 



Figure 7.5: A planar four-bar linkage and its joint configuration space. 



Figure 7.6: A planar five-bar linkage. 


where 


ol = 2 A 3 X 4 — 2LiL 3 cos 9 (7.23) 

P = —2L\L 3 sin# (7.24) 

7 = L 2 — L 2 — L 2 — Lf + 2 L 1 L 4 cos 9. (7.25) 

The existence and uniqueness of solutions to the above depends on the link 
lengths Li,..., L 4 . In particular, a solution will fail to exist if j 2 < a 2 + f3 2 . 
The figure depicts the input-output graph for the choice of link lengths L\ = 4, 
L 2 = 4, L 3 = 3, L 4 = 2. For this set of link lengths, 9 and (f> both range from 0 
to 2-7T. 

One of the distinctive features of this graph is the bifurcation point P 
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Figure 7.7: Configuration space singularities of the planar five-bar linkage. 


as indicated in the figure. Here two branches of the curve meet, resulting in a 
self-intersection of the curve with itself. If the four-bar is in the configuration 
indicated by P, it has the choice of following either branch. At no other point 
in the four-bar’s C-space does such branching occur. 

We now turn to the five-bar linkage. The kinematic loop constraint equations 
can be written 

L\ cos d\ +...-(- Lj\ cos(0i + O 2 T @3 T 0 4 ) = P 5 (7.26) 

Pi sin ^-K.. + T 4 sin( 6 >i + 6 > 2 + 0 3 + 0 4 ) = 0 (7.27) 

where we have eliminated in advance the joint variable 05 from the loop closure 
conditions. Writing these two equations in the form /(0 1 ,... , 64 ) = 0, where 
/ : K 4 —> R 2 , the configuration space can be regarded as a two-dimensional 
surface in R 4 . Like the bifurcation point of the four-bar linkage, self-intersections 
of the surface can also occur. At such points the constraint Jacobian loses rank. 
For the five-bar, any point 0 at which 

rank (|^(0)) < 2 (7-28) 

corresponds to what we call a configuration space singularity. Figure 7.7 
illustrates the possible configuration space singularities of the five-bar. Notice 
that so far we have made no mention of which joints of the five-bar are actuated, 
or where the end-effector frame is placed. The notion of a configuration space 
singularity is completely independent of the choice of actuated joints, or where 
the end-effector frame is placed. 

We now consider the case when two joints of the five-bar are actuated. Re¬ 
ferring to Figure 7.8, the actuated joints are indicated by filled circles. Under 
normal operating conditions, the motions of the actuated joints can be indepen¬ 
dently controlled. Alternatively, locking the actuated joints should immobilize 
the five-bar and turn it into a rigid structure. 

For the nondegenerate actuator singularity shown on the left of Fig¬ 
ure 7.8, rotating the two actuated joints oppositely and outward will pull the 
mechanism apart, and rotating them oppositely and inward would either crush 
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Figure 7.8: Actuator singularities of the planar five-bar linkage: the left is 
nondegenerate, while the right is degenerate. 


the inner two links, or cause the center joint to unpredictably buckle upward 
or downward. For the degenerate actuator singularity shown on the right, 
even when the actuated joints are locked in place, the inner two links are free 
to rotate. 

The reason for classifying these singularities as actuator singularities is 
that, by relocating the actuators to a different set of joints, such singularities can 
be eliminated. For both the degenerate and nondegenerate actuator singularities 
of the five-bar, relocating one of the actuators to one of the three passive joints 
eliminates the singularity. 

Visualizing the actuator singularities of the planar five-bar is straightfor¬ 
ward enough, but for more complex spatial closed chains this may be difficult. 
Actuator singularities can be characterized mathematically by the rank of the 
constraint Jacobian. As before, write the kinematic loop constraints in differ¬ 
ential form, 


H(q)q=[H a (q) H p (q) 



(7.29) 


where q a G R a is the vector of actuated joints, and q p G is the vector of 
passive joints. It follows that H(q) G Rp x (“+p) and that H p (q ) is a pxp matrix. 
With the above definitions, we have the following: 


• If rank H p {q) < p , then q is an actuator singularity. Distinguishing be¬ 
tween degenerate and nondegenerate singularities involves additional 
mathematical subtleties, and relies on second-order derivative information 
that we do not pursue further here. 


• If rank H{q) < p , then q is a configuration space singularity. Note 
that under this condition H p (q) is also singular (the converse is not true, 
however). The configuration space singularities can therefore be regarded 
as the intersection of all possible actuator singularities obtained over all 
possible combinations of actuated joints. 
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Figure 7.9: End-effector singularity of the planar five-bar linkage. 


The final class of singularities depends on the choice of end-effector frame. 
For the five-bar, ignore the orientation of the end-effector frame, and focus 
exclusively on its x-y location. Figure 7.9 shows the five-bar in an end-effector 
singularity for the given choice of end-effector location. Note that velocities 
along the indicated line are not possible in this configuration, similar to the 
case for singularities for open chains. To see this, consider the effective 2R open 
chain created by the rightmost joint, the link connecting it to the platform, 
the joint on the platform, and the effective link connecting the platform joint 
to the end-effector frame. Since the two links of the 2R robot are aligned, the 
end-effector frame can have no component of motion along the direction of the 
links. 

End-effector singularities are independent of the choice of actuated joints. 
They can be mathematically characterized as follows. Choose any valid set of 
actuated joints q a such that the mechanism is not at an actuator singularity. 
Write the forward kinematics in the form 

f{q a ) = T sb . (7.30) 

One can then check for rank deficiencies in the Jacobian of /, as was done for 
open chains, to determine the presence of an end-effector singularity. 

7.4 Summary 

• Any kinematic chain that contains one or more loops is called a closed 
chain. Parallel mechanisms are a class of closed chain that are char¬ 
acterized by two platforms—one moving and one stationary—connected 
by several legs; the legs are typically open chains, but can themselves be 
closed chains. Compared to open chains, the kinematic analysis of closed 
chains is complicated by the fact that only a subset of joints are actuated, 
and the joint variables must satisfy a number of loop-closure constraint 
equations, which may or may not be independent depending on the con¬ 
figuration of the mechanism. 
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• For a parallel mechanism with equal numbers of actuators and degrees of 
freedom, the inverse kinematics problem involves finding, from the given 
position and orientation of the moving platform, the joint coordinates of all 
the actuated joints. For well-known parallel mechanisms like the planar 3 x 
RPR and spatial Stewart-Gough platform, the inverse kinematics admits 
unique solutions. 

• For a parallel mechanism with equal numbers of actuators and degrees 
of freedom, the forward kinematics problem involves finding the position 
and orientation of the moving platform given coordinates for all the ac¬ 
tuated joints. For well-known parallel mechanisms like the 3 x RPR and 
the spatial Stewart-Gough platform, the forward kinematics usually ad¬ 
mits multiple solutions. In the case of the most general Stewart-Gough 
platform, a maximum of 40 solutions are possible. 

• The differential kinematics of a closed chain relates velocities of the ac¬ 
tuated joints to the linear and angular velocities of the moving platform. 
For a closed chain consisting of n one-dof joints, with m actuators and 
degrees of freedom, let 9 a £ R m denote the vector of actuated joints and 
9 p £ R” -rn denote the vector of passive joints. The kinematic loop-closure 
constraints are described by an equation of the form h(9 a ,0 p ) = 0, where 
g : R” — > K n-m . The forward kinematics can be expressed in the form 
f(9 a ) = T , where / : R m — > SE(3). The differential kinematics involves 
derivatives of both / and g with respect to 9 a and 9 p . For platforms like 
the Stewart-Gough platorm, the differential kinematics can also be ob¬ 
tained from a static analysis, by exploiting the fact that just as for open 
chains, the external forces T at the end-effector are related to the joint 
forces or torques r by r = J T P. 

• Singularities for closed chains can be classified into three types: (i) config¬ 
uration space singularities at self-intersections of the configuration space 
surface (also called bifurcation points for one-dimensional configuration 
spaces); (ii) nondegenerate actuator singularities when the actuated joints 
cannot be independently actuated, and degenerate actuator singularities 
when locking all joints fails to make the mechanism a rigid structure; 
and (iii) end-effector singularities when the end-effector loses one or more 
degrees of freedom of motion. Configuration space singularities are in¬ 
dependent of the choice of actuated joints, while actuator singularities 
depend on which joints are actuated. End-effector singularities depend on 
the placement of the end-effector frame, but do not depend on the choice 
of actuated joints. 

7.5 Notes and References 


Several methods exist for finding all solutions to systems polynomial equations, 
e.g., methods based on dialytic elimination, Grobner bases, etc. Of particular 
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note is the work of Raghavan and Roth [101], who show that there can be at 
most forty solutions to the forward kinematics of the general 6-6 Stewart-Gough 
Platform, and Husty [44], who develops a computational algorithm to find all 
forty solutions. 
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7.6 Exercises 



Figure 7.10: 3xRPR planar parallel mechanism. 


1. In the 3xRPR planar parallel mechanism of Figure 7.10, the prismatic joints 
are actuated. Define a* £ R 2 to be the vector from the fixed frame origin O 
to joint Aj, * = 1,2, 3, expressed in fixed frame coordinates. Define 6, ; £ R 2 to 
be the vector from the moving platform frame origin P to joint Bj, * = 1,2, 3, 
defined in terms of the moving platform frame coordinates. 

(a) Solve the inverse kinematics. 

(b) Derive a procedure to solve the forward kinematics. 

(c) Is the configuration shown an end-effector singularity? Explain your an¬ 
swer by examining the inverse kinematics Jacobian. Is this also an actuator 
singularity? 

2. For the 3xRPR planar parallel mechanism shown in Figure 7.11(a), let </> 
be the angle measured from the {b}-frame x-axis to the {s}-frame x-axis, and 
p £ R 2 be the vector from the {s}-frame origin to the {b}-frame origin, ex¬ 
pressed in {s}-frame coordinates. Let a; £ R 2 be the vector from the {s}-frame 
origin to the three joints fixed to ground, * = 1,2,3 (note that two of the joints 
are overlapping), expressed in {s}-frame coordinates. Let bi £ R 2 be the vector 
from the {b}-frame origin to the three joints attached to the moving platform, 
* = 1,2,3 (note that two of the joints are overlapping), expressed in {b}-frame 
coordinates. Denote the leg lengths by 9 1: 9 2 , 9 3 as shown. 

(a) Derive a set of independent equations relating (<p,p) and (Q\, 9 2l 9 3 ). 

(b) What is the maximum possible number of forward kinematics solutions? 

(c) Assuming static equilibrium, given joint torques r = (1,0,—1) applied at 
joints (9i,9 2 ,0 3 ), find the force ( f x ,f y ) and the moment m z £ R generated at 













7.6. Exercises 


235 


the end-effector frame {b}. Express your (f x ,f y ) in fixed frame coordinates, 
(d) Now construct a mechanism with three connected 3xRPR parallel mecha¬ 
nisms as shown in Figure 7.11(b). What is the dof of this mechanism? 




Figure 7.11: 3XRPR planar parallel mechanism and truss structure. 


3. For the 3xRRR planar parallel mechanism shown in Figure 7.12, let (f> be 
the orientation of the end-effector frame and pgK 2 be the vector p expressed 
in fixed frame coordinates. Let € R 2 be the vector a i expresed in fixed frame 
coordinates and £ R 2 be the vector b^ expressed in the moving body frame 
coordinates. 

(a) Derive a set of independent equations relating ( 4 >,p) and (61,62,63). 

(b) What is the maximum possible number of inverse and forward kinematic 
solutions for this mechanism? 

4. Figure 7.13 shows a six-bar linkage in its zero position. Let ( p x , p y ) be the 
position of the {b}-frame origin expressed in {s}-frame coordinates, and (j) be 
the orientation of the {b} frame. The inverse kinematics problem is defined as 
finding the joint variables (9,i/j) given (p x ,p y ,(f>). 

(a) In order to solve the inverse kinematics problem, how many equations are 
needed? Derive these equations. 
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(b) Assume joints A, D, and E are actuated. Determine if the configuration 
shown in Figure 7.13 is an actuator singularity by analyzing an equation of the 
form 


[ H a H p 


Qa 

Qp 


= 0 , 


where q a is the vector of actuated joints and q p is the vector of passive joints, 
(c) Suppose instead that joints A, B, and D are actuated. Find the forward 
kinematics Jacobian J a in V s = J a q a , where V s is the velocity twist of the {b}- 
frame expressed in {s}-frame coordinates, and q a is the vector of actuated joint 
rates. 


5. Consider the 3xPSP spatial parallel mechanism of Figure 7.14. 

(a) What is the degrees of freedom of this mechanism? 

(b) Let i? s f,=Rot(z, 0)Rot(y, </>)Rot(x, t/>) be the orientation of the moving body 
frame {b}, and p s b = (x,y,z) € R 3 be the vector from the {s}-frame origin to 
the {b}-frame origin (both R s b and p s b are expressed in {s}-frame coordinates). 
The vectors Si, b;, di, i = 1,2,3, are defined as shown in the figure. Derive a set 
of independent kinematic constraint equations relating {6,(j),il),x,y, z) and the 
vectors defined. 

(c) Given values for ( x,y,z ), is it possible to solve for the vertical prismatic 
joint values Si , where Sj = ||dj||, i = 1,2, 3? If so, derive an algorithm for doing 
so. 







7.6. Exercises 


237 


F i E i D 



Figure 7.13: A six-bar-linkage. 



Figure 7.14: 3xPSP spatial parallel manipulator. 


6 . The Eclipse mechanism of Figure 7.15 is a six-dof parallel mechanism whose 
moving platform is capable of ±90° orientations with respect to ground, and 
also of rotating 360° about the vertical axis. 

(a) Derive the forward and inverse kinematics. How many forward kinematic 
solutions are there for general nonsingular configurations? 
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Figure 7.16: The 3xUPU mechanism. 


(b) Find and classify all singularities of this mechanism. 

7. For the Delta robot of Figure 2.8, derive the following: 

(a) The forward kinematics. 

(b) The inverse kinematics. 

(c) The Jacobian J a (assume the revolute joints at the fixed base are actuated). 

(d) Identify all actuator singularities of the Delta robot. 































7.6. Exercises 


239 


8 . In the 3xUPU platform of Figure 7.16, the axes of the universal joints are 
attached to the fixed and moving platforms in the sequence indicated, i.e., the 
first axis is attached orthogonally to the fixed base, while the fourth axis is 
attached orthogonally to the moving platform. Derive the following: 

(a) The forward kinematics. 

(b) The inverse kinematics. 

(c) The Jacobian J a (assume the revolute joints at the fixed base are actuated). 

(d) Identify all actuator singularities of this robot. 

(e) If you can, build a mechanical prototype and see if the mechanism behaves 
as predicted by your analysis. 
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Chapter 8 


Dynamics of Open Chains 


In this chapter we study once again the motion of open-chain robots, but this 
time taking into account the forces and torques that cause it; this is the subject 
of robot dynamics. The associated dynamic equations—also referred to as 
the equations of motion —are a set of second-order differential equations of 
the form 

T = M(0)9 + h(9,9), (8.1) 

where 9 £ M" is the vector of joint variables, r £ M" is the vector of joint forces 
and torques, M(0) £ R nxn is a symmetric positive-definite matrix called the 
mass matrix, and h(0,0 ) £ are forces that lump together centripetal and 
Coriolis, gravity, and friction terms that depend on 9 and 9. One should not be 
deceived by the apparent simplicity of these equations; even for “simple” open 
chains, e.g., those with joint axes either orthogonal or parallel to each other, 
M{9) and h(0,0) can be extraordinarily complex. 

Just as a distinction was made between a robot’s forward and inverse kine¬ 
matics, it is also customary to distinguish between a robot’s forward and in¬ 
verse dynamics. The forward problem is the problem of determining the 
robot’s acceleration 9 given the state (0,0) and the joint forces and torques, 

0 = M- 1 {e)(r-h{0,9) y j, (8.2) 

and the inverse problem is finding the joint forces and torques r corresponding 
to the robot’s state and a desired acceleration, i.e., Equation (8.1). 

A robot’s dynamic equations are typically derived in one of two ways: by 
a direct application of Newton and Euler’s dynamic equations for a rigid body 
(often called the Newton-Euler formulation), or by the Lagrangian dy¬ 
namics formulation deriving from the kinetic and potential energy of the robot. 
The Lagrangian formalism is conceptually elegant and quite effective for robots 
with simple structures, e.g., with three or fewer degrees of freedom. However, 
the calculations can quickly become cumbersome for robots with more degrees 
of freedom. For general open chains, the Newton-Euler formulation leads to ef¬ 
ficient recursive algorithms for both the inverse and forward dynamics that can 
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also be assembled into closed-form analytic expressions for, e.g., the mass ma¬ 
trix M(0) and other terms in the dynamics equation (8.1). The Newton-Euler 
formulation also takes advantage of tools we have developed so far in this book. 

In this chapter we study both the Lagrangian and Newton-Euler dynamics 
formulations for an open-chain robot. 


8.1 Lagrangian Formulation 


8.1.1 Basic Concepts and Motivating Examples 


The first step in the Lagrangian formulation of dynamics is to choose a set of in¬ 
dependent coordinates q G R™ that describes the system’s configuration, similar 
to what was done in the analysis of a robot’s configuration space. The coor¬ 
dinates q are called generalized coordinates. Once generalized coordinates 
have been chosen, these then define another set of coordinates / G R" called 
generalized forces. The forces / and the coordinates q are dual to each other 
in the sense that the inner product f T q corresponds to power. A Lagrangian 
function C(q, q) is then defined as the overall system’s kinetic energy K.(q, q) mi¬ 
nus the potential energy V(q). The equations of motion can now be expressed 
in terms of the Lagrangian as follows: 


d^dC _ dC 
dt dq dq ’ 


(8.3) 


These equations are also referred to as the Euler-Lagrange equations with 
external forces . 1 The derivation can be found in dynamics texts. 

We illustrate the Lagrangian dynamics formulation through two examples. 
In the first example, consider a particle of mass m constrained to move on 
a vertical line. The particle’s configuration space is this vertical line, and a 
natural choice for generalized coordinate is the height of the particle, which we 
denote by the scalar variable x G M. Suppose the gravitational force mg acts 
downward, and an external force / is applied upward. By Newton’s second law, 
the equation of motion for the particle is 


/ — mg = mi. 


(8.4) 


We now apply the Lagrangian formalism to derive the same result. The kinetic 
energy is mi 2 /2, the potential energy is rngx, and the Lagrangian is 

C(x, x ) = JC(x, x) — V{x) = ^mi 2 — mgx. (8.5) 


The equation of motion is then given by 


/ 


d dC dC 
dt dx dx 


mi + mg, 


( 8 . 6 ) 


which matches Equation (8.4). 

1 The external force / is zero in the standard form of the Euler-Lagrange equations. 
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Figure 8.1: (Left) A 2R open chain in gravity. (Right) At 0 = (0,7r/2). 


We now derive the dynamic equations for a planar 2R open chain moving 
in the presence of gravity (Figure 8.1). The chain moves in the x-y plane, with 
gravity g acting in the —y direction. Before the dynamics can be derived, the 
mass and inertial properties of all the links must be specified. To keep things 
simple the two links are modeled as point masses inx and m 2 concentrated at 
the ends of each link. The position and velocity of the mass of link 1 are then 
given by 


Xi 


L\ cos 9\ 

. y 1 . 


L 1 sin0! 

Xi 


—Li sin 0i 

. in . 


Li cos 0i 


while those of the link 2 mass are given by 


x 2 


L\ cos 6\ + L2 cos(#i + 62) 



02 . 


L\ sin 0\ L2 sin($i 62) 



x 2 


—L\ sin 9 1 — Z /2 sin(0i + O2) 

— L 2 sin(0i + 0 2 ) 

' 9 X ' 

02 _ 


L\ cos 9\ + L2 cos(^i + 62) 

L 2 COS (0i + 0 2 ) 

. 9 2 _ 


We choose the joint coordinates 9 = (0i,0 2 ) to be the generalized coordinates. 
The generalized forces r = (ti,t 2 ) then correspond to joint torques (since t t 9 
corresponds to power). The Lagrangian £(0, 9) is of the form 

2 

£(M) = ^(ACi - Pi), (8-7) 

where the link kinetic energy terms /C 1 and /C 2 are 
/Ci = + yl) = \miL\6l 

K-2 = ^m 2 {±l + yl) 

= ’tt ((^+2L 1 L 2 cos 9 2 + Ll)9\ + 2{L 2 2 + cos 9 2 )9 1 9 2 + L 2 2 9^ , 
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and the link potential energy terms V\ and V2 are 


Vi = mi0?/i = migii sin6>! 

V2 = m 2 0j/2 = m 2 0 (+i sin + L 2 sin(0i + 0 2 )). 


The Euler-Lagrange equations ( 8 . 3 ) for this example are of the form 


d_dC 
dt dOi 


dC 

W 


i = 1,2. 


( 8 . 8 ) 


The dynamic equations for the 2 R planar chain follow from explicit evaluation 
of the right-hand side of (8.8) (we omit the detailed calculations, which are 
straightforward but tedious): 

T\ = (m\L\ + m2 (+1 + 2+i+ 2 cos 62 + + 2 )) 0\ + 

m 2 (+i+ 2 cos 02 + + 2 )02 ~ m 2 +i+2 sin 0 2 (20102 + 0 2 ) + 

(mi + m 2 )+i0cos0i + m 2 0+ 2 cos(0i +02) 
t 2 = m 2 (+i+ 2 cos0 2 ++ 2 )0i + m 2 +i02 + m 2 -bi+20i sin0 2 + 
m 2 0+ 2 cos(0i + 0 2 ). 


We can gather terms together into an equation of the form 


with 

M( 0 ) 


c(0,0) 

9 ( 0 ) 


t = M( 6)9 + c( 0 , 0 ) + 5(0), ( 8 . 9 ) 

v_ v _✓ 

h(O,0 ) 


mi+ ai 2 (+1 + 2+1+2 cos 0 2 + + 2 ) m 2 (-^1+2 cos 0 2 + + 2 ) 
m 2 (+1+2 cos 0 2 ++2) m 2 +f 

—m 2 +i+ 2 sin 0 2 (20 i 0 2 + 0 2 ) 
m 2 +i+ 2 0i sin 0 2 

(mi + m 2 )+i0 cos 0i + m 2 0+ 2 cos(0i + 0 2 ) 
m 2 0+2 cos(0i + 0 2 ) 


where M{ 9 ) is the symmetric positive-definite mass matrix, c( 0 , 0 ) is a vector 
of Coriolis and centripetal torques, and g( 9 ) is a vector of gravitational torques. 
These reveal that the equations of motion are linear in 0 , quadratic in 0 , and 
trigonometric in 0 . This is true in general for serial chains containing revolute 
joints, not just the 2 R robot. 

The M( 0)0 + c( 0 , 0 ) portion of Equation ( 8 . 9 ) could have been derived by 
writing /j = m,a,; for each point mass, where the accelerations a, are written 
in terms of 0 by differentiating the expressions for (£1,2/1) and (£2,2/2) given 










8 . 1 . Lagrangian Formulation 


245 


above: 



fx 1 


Xi 


— L\0^C\ — Liflisi 

fl = 

fyl 

= mi 

2/1 

= mi 

—Li^si -|- L161C1 


/z! 


Zi 


0 


h = m 2 


—Li 9 \c\ — T 2 ( 0 i + 0 2 ) 2 C12 — Lj lSl — L 2 ( 0 i + 62)812 

— Li 9 iSi — T 2 ( 0 i + 02) 2 Si2 + L1&1C1 + L 2 ( 0 i + 62)012 > 

0 


( 8 . 10 ) 


( 8 . 11 ) 


where Si 2 indicates sin( 0 i + 0 2 ), etc. Defining rn as the vector from joint 1 to 
mi, ri 2 as the vector from joint 1 to m 2 , and r 22 as the vector from joint 2 to 
m 2 , the moments in world-aligned frames {«} attached to joints 1 and 2 can be 
expressed as mi = rn x f\+r 12 x / 2 and m 2 = r 22 x / 2 . (Note that joint 1 must 
provide torques to move both mi and m 2 , since both masses are outboard of 
joint 1 , but joint 2 only needs to provide torque to move m 2 .) The joint torques 
ri and t 2 are just the third elements of m\ and m 2 , i.e., the moments about the 
z i axes out of the page, respectively. 

In (x, y) coordinates, the accelerations of the masses are written simply as 
second time-derivatives of the coordinates, e.g., (x 2 ,y 2 ). This is because the 
x-y frame is an inertial frame. The joint coordinates ( 0 i, 0 2 ) are not in an 
inertial frame, however, so accelerations are expressed as a sum of terms that 
are linear in the second derivatives of joint variables, 6 , and quadratic of the 
first derivatives of joint variables, 9 T 8 1 as seen in Equations ( 8 . 10 ) and ( 8 . 11 ). 
Quadratic terms containing 0 2 are called centripetal terms, and quadratic 
terms containing 0j0j are called Coriolis terms. In other words, 9 = 0 does not 
mean zero acceleration of the masses, due to the centripetal and Coriolis terms. 

To better understand the centripetal and Coriolis terms, consider the arm 
at the configuration ( 0 i, 0 2 ) = (0,7r/2), i.e., cos 0 i = sin( 0 i + 8 2 ) = 1 , sin 0 i = 
cos( 0 i + Q 2 ) = 0 . Assuming 0 = 0 , the acceleration (x 2 , y 2 ) of m 2 from Equa¬ 
tion (8.11) can be written 


X 2 


-W8\ 

. y*. 


—T 2 0i — T 2 0| 


V,___✓ 


centripetal terms 


0 

— 2L2O162 

X- v _✓ 

Coriolis terms 


Figure 8.2 shows the centripetal acceleration a cen ti = (—Ti 0 p — T 2 0 i) when 
0 2 = 0; the centripetal acceleration a cen t2 = (0, —T 2 0|) when 9 i = 0; and the 
Coriolis acceleration a cor = ( 0 , — 2 L 2 0 i 0 2 ) when both 8 \ and 0 2 are positive. 
As illustrated in Figure 8 . 2 , each of the centripetal accelerations a ce nti pulls m 2 
toward joint i to keep m 2 rotating about about the center of the circle defined 
by joint i? Therefore a cen ti creates zero torque about joint i. The Coriolis 
acceleration a cor in this example passes through joint 2, so it creates zero torque 
about joint 2 , but it creates negative torque about joint 1 . Intuitively, the 

2 Without this centripetal acceleration, and therefore centripetal force, the mass m 2 would 
fly off along a tangent to the circle. 
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Figure 8.2: Accelerations of m 2 when 9 = (0, 7 t/2 ) and 9 = 0. (Left) The 
centripetal acceleration a cent i = {—Li9\,—L 2 9\) of m 2 when 9 2 = 0. (Middle) 
The centripetal acceleration a cen t 2 = (0, —L 2 9 2 ) °f m 2 when 9\ = 0. (Right) 
When both joints are rotating with 0i > 0, the acceleration is the vector sum of 
^centi and a cen t 2 and the Coriolis acceleration a cor = (0, — 2L 2 9i9 2 ). 

torque about joint 1 is negative because m 2 gets closer to joint 1 (due to joint 
2’s motion). Therefore the inertia due to m 2 about the zi-axis is dropping, and 
therefore the positive momentum about joint 1 drops while joint l’s speed is 
constant, and therefore joint 1 must apply a negative torque, since torque is 
defined as the rate of change of momentum. Otherwise f?i would increase as m 2 
gets closer to joint 1 , just as a skater’s rotation speed increases as she pulls in 
her outstretched arms while doing a spin. 

8.1.2 General Formulation 

We now describe the Lagrangian dynamics formulation for general n-link open 
chains. The first step is to select a set of generalized coordinates 9 £ R” for 
the configuration space of the system. For open chains all of whose joints are 
actuated, it is convenient and always possible to choose 9 to be the vector of 
joint values. The generalized forces will then be denoted r £ R n . If 9i is a 
revolute joint, r,; will correspond to a torque, while if 9i is a prismatic joint, t* 
will correspond to a force. 

Once 9 has been chosen and the generalized forces r identified, the next step 
is to formulate the Lagrangian C(9,9) as 

£{9,9)=1C{9,9)-P(9), (8.12) 

where JC(9, 9) is the kinetic energy and V{9) the potential energy of the overall 
system. For rigid-link robots the kinetic energy can always be written in the 
form 

1 n n 1 

m = i'Z'E m ij (9)9i9 j = -9 t M(9)9, (8.13) 

*=1 i=i 

where mij(9) is the {i.j) element of the nxn mass matrix M{9)\ a constructive 
proof of this assertion is provided when we examine the Newton-Euler formula¬ 
tion. 
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The dynamic equations are analytically obtained by evaluating the right- 
hand side of 


- _ Y ■ _ , 

~did0 i M’ ,n 


(8.14) 


With the kinetic energy expressed as in Equation (8.13), the dynamics can be 
written explicitly as 


n 


Y + YY T m{0)0j0 k + Q^, i = 

j =i j =i fe=l 2 


(8.15) 


where the ^-*,(0), known as the Christoffel symbols of the first kind, are 
defined as 


r«*(0) = 2 


1 / 9m,- 


9m. 




dm 


'jk 


dO k 


d6 , 




(8.16) 


This shows that the Christoffel symbols, which generate the Coriolis and cen¬ 
tripetal terms c(0, 6), are derived from the mass matrix M(9). 

As we have already seen, the equations (8.15) are often gathered together in 
the form 


T = M(9)6 + c(9,9) + g{9 ) or M(9)9 + h(0,9), 


where g{9) is simply dV/d9. 

To be more explicit that the Coriolis and centripetal terms are quadratic in 
the velocity, we could instead use the form 


r = M(9)8 + 9 t T( 8)8 + g(9 ), 


(8.17) 


where T(0) is an n x n 
as 


x n matrix, and the product 9 t T(9)9 should be interpreted 


8 t T{9)8 


8 t T 1 (8)9 ' 

8 t T 2 {8)8 


. 8 T Y n {0)8 _ 


where Ti(0) is an n x n matrix with ( j , k ) entries T^fe. 
It is also common to see the dynamics written as 


T = M(9)9 + C{9 1 9)8 + g{9 ), 


where C(8,9) £ R” xn is called the Coriolis matrix, with (i,j) entries 


Cij(8,0) = Y r <3fc(0)ftfc- (8-1-8) 

fe=i 


The following property, referred to as the passivity property, turns out to 
have important ramifications in proving the stability of certain robot control 
laws, as we will see in Chapter 11. 
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Proposition 8.1. The matrix M{9) — 2(7(0,0) £ R” xn , where M{6) £ R nxn 
is the mass matrix and M{9) its time derivative, and C{9,9) £ R nxra is the 
Coriolis matrix as defined in Equation (8.18), is skew symmetric. 

Proof. The (i,j) component of M — 2 C is 


,-(*)-2 ClJ (e,0) = j^^k-^k-^k 


fc=i 


dO k 


(36, 


_ dm-kj g dmik ^ 


86 , 


d(). 


fc= i “ 0 

By switching the indices i and j, it can be seen that 

rhji(9) - 2cji(6,9) = -( rhij(9) - 2 dj(9,9)), 

thus proving that (M — 2 C) T = —(M — 2 C) as claimed. 


dm k j a 


89i 


9b 


□ 


8.1.3 Understanding the Mass Matrix 

The kinetic energy \0 T M(6)9 is a generalization of the familiar | mv T v for a 
point mass. The fact that the mass matrix M(0) is positive definite, meaning 
that 6 t M{9)9 > 0 for all 9 ^ 0 , is a generalization of the fact that the mass of 
a point mass is always positive, m > 0. In both cases, if the velocity is nonzero, 
the kinetic energy must be positive. 

For a point mass with dynamics expressed in Cartesian coordinates as / = 
mx , the mass is independent of the direction of acceleration, and the acceleration 
x is always “parallel” to the force, in the sense that x is a scalar multiple of 
/. A mass matrix M(9), on the other hand, presents different effective mass in 
different acceleration directions, and 9 is not generally a scalar multiple of r, 
even when 0 = 0. To visualize the direction dependence of the effective mass, 
we can map a unit ball of joint accelerations {6 \ 0 T 9 = 1} through the mass 
matrix M(9) to generate a joint force/torque ellipsoid when the mechanism is 
at rest (0 = 0). An example is shown in Figure 8.3 for the 2R arm of Figure 8.1, 
where L\ = L 2 = mi = m 2 = 1. This torque ellipsoid can be interpreted as a 
direction-dependent mass ellipsoid: to get the same joint acceleration magnitude 
|| 0 ||, you must apply a larger torque to joint 1 than you would have to apply 
to joint 2. This effect is reduced as link 2 folds back on link 1, reducing the 
distance between joint 1 and m 2 , as shown in Figure 8.3. The direction of the 
principal axes of the mass ellipsoid are given by the eigenvectors v\ of M(9) and 
the lengths of the principal semi-axes are given by the corresponding eigenvalues 
A j. The acceleration 0 is only a scalar multiple of r when r is along one of the 
principal axes of the ellipsoid. 

It is easier to visualize the mass matrix if it is represented as an effective 
mass of the end-effector. In other words, if you grabbed the endpoint of the 2R 
robot, how massy would it feel depending on the direction you applied force to 
it? Let us denote the effective mass matrix at the end-effector as A(0), and the 
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Figure 8.3: (Bold lines) A unit ball of accelerations in 9 maps through the mass 
matrix M{6) to a torque ellipsoid that depends on the configuration of the 2R 
arm. These torque ellipsoids may be interpreted as mass ellipsoids. (Dotted 
lines) A unit ball in r maps through M~ 1 (9) to an acceleration ellipsoid. 


velocity of the end-effector as V = {x,y) T . We know that the kinetic energy of 
the robot must be the same regardless of the coordinates we use, so 

l9 t M(9)9 = \v t A (6)V. (8.19) 

Assuming the Jacobian J{9) satisfying V = J(6)6 is invertible, then Equa¬ 
tion (8.19) can be rewritten as 

V t AV = {J~ 1 V) T M(J-V) 

= V t (J~ t MJ~ 1 )V. 

In other words, the end-effector mass matrix is 

A(0) = J- T (9)M(6)J- 1 {e). (8.20) 

Figure 8.4 shows the end-effector mass ellipsoids, with principal axes given by 
the eigenvectors of A(0) and principal semi-axis lengths given by the eigenval¬ 
ues, for the same two 2R robot configurations as in Figure 8.3. The endpoint 
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Figure 8.4: (Bold lines) A unit ball of accelerations in (x,y) maps through the 
end-effector mass matrix A(0) to an end-effector force ellipsoid that depends 
on the configuration of the 2R arm. For the top right configuration, a force in 
the f y direction exactly feels both masses mi and m 2 , while a force in the f x 
direction feels only m 2 . (Dotted lines) A unit ball in / maps through A -1 (0) to 
an acceleration ellipsoid. 


acceleration is only a scalar multiple of the force if the force is along one of the 
principal axes of the ellipsoid. 

The change in apparent endpoint mass as a function of the configuration of 
the robot is an issue for robots used as haptic displays. One way to mitigate 
this issue is to make the mass of the links as small as possible. 

Note that these ellipsoidal interpretations of the relationship between forces 
and accelerations are only relevant at zero velocity, where there are no Coriolis 
or centripetal terms. 

8.1.4 Lagrangian Dynamics vs. Newton-Euler Dynamics 

In the rest of this chapter, we focus on the Newton-Euler recursive method 
for calculating robot dynamics. Using the tools we have developed so far, the 
Newton-Euler formulation allows computationally efficient computer implemen¬ 
tation, particularly for robots with many degrees of freedom, without the need 
for differentiation. The resulting equations of motion are, and must be, identical 
to those derived using the energy-based Lagrangian method. 
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The Newton-Euler method builds on the dynamics of a single rigid body, so 
we begin there. 

8.2 Dynamics of a Single Rigid Body 

8.2.1 Classical Formulation 

Consider a rigid body consisting of a number of rigidly connected point masses, 
where point mass i has mass m, and the total mass is m = JT m;. Let r,; = 
(xi, yi, Zi) T be the fixed location of mass i in a body frame {b}, where the origin 
of this frame is the unique point such that 

^rtqr-i = 0. 


This point is known as the center of mass. If some other point is erroneously 
chosen as the origin, then the frame {b} should be moved to the center of mass 
at (1/nr) JT and the rj’s recalculated in this center-of-mass frame. 

Now assume that the body is moving with a body twist 14 = ( ui b ,v b ), and 
let pi (t) be the time-varying position of rrq , initially located at r, , in the inertial 
frame {b}. Then 


Pi = v b + U) b x Pi 


Pi = Vb + 


d 

— 0J b X Pi+U} b X 
at 



= V b + UJ b X Pi + LO b X (v b + U b X Pi). 


Plugging in pi = r* and using our skew-symmetric notation, we get 
pi = Vb + [u b \ri + [uj b ]v b + [ tjJ b ] 2 ri. 

Taking as a given that /) = m ipi for a point mass, the force acting on m, is 
fi = m i(v b + [uj b \ri + [tu b ]v b + [u b ] 2 ri), 
which implies a moment 

mi = [ri\fi. 

The total force and moment acting on the body is expressed as the wrench T b \ 

E i m i 
E i fi 

To simplify the expressions for f b and m b , keep in mind that E,: m * r i = 0 
(and therefore Ei m *[ r j] = 0), and for a,b £ M 3 , [a] = — [a] T , [a]6 = — [6]a, and 
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[a][&] = ([6][a]) T . Focusing on the linear dynamics, 

/ b = £ m t (v b + [uj b \ri + [oj b ]v b + [wt,] 2 ^) 

i 

= + [u b ]v b ) - 

i 

= + [uj b ]v b ) 

i 

= m(v b + [uj b ]v b ). (8-21) 



Now focusing on the rotational dynamics, 


m b 


y. + [u> b ]ri + [u} b ]v b + [u} b ] 2 ri) 

J) 0 

y^m j\rtfb b 

+ 5 Zm i [r i ]([w 6 ]r i + K] 2 ri) 

i 

ym t (-[n\ 2 u; b - [ri] r [w b ] T [r i ]w 6 ) 

i 

{—[ri] 2 uj b — [uj b \[ri] 2 u} b ) 

i 

y mdc } 2 j ih b + [uj b ] y miln } 2 j uj b 

X b UJ b [^b]-FbU^b, 


( 8 . 22 ) 


where I b = — Ei n h[ r i] 2 £ R 3x3 is the body’s rotational inertia matrix. In 
Equation (8.22), note the presence of a term linear in the angular acceleration, 
X b Lu b: and a term quadratic in the angular velocities, {uj b ]X b ijj b ^ just as we saw 
for mechanisms in Section 8.1. Also, X b is symmetric and positive definite, just 
like the mass matrix for a mechanism, and the rotational kinetic energy is given 
by the quadratic 


K .: = 


1 

2 


uj b X b uj b . 


One difference is that X b is constant, whereas the mass matrix M(9) changes 
with the configuration of the mechanism. 

Writing out the individual entries of X b , we get 


X b 


E Mvf + z 2 ) 

- E WiXiVi 

- Em i x i z i 

T T T 

-L'XX -L'X'y -L'XZ 

7 7 J 

-L'xy -‘-'yy yz 

7 7 7 

-L'xz -^yz -L'zz 


- E m iXiUi 

E m i( x i + z i) 

- E W-iViZi 


- E m t XiZi 

- E W-iViZi 

Em i(x 2 + y 2 ) 
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The summations can be replaced by volume integrals over the body B , with the 
point masses tip replaced by a mass density function p(x,y,z): 


T — 

-‘-'T.cr. — 


T = 
-^yy 


T = 

-‘-' 7 . 7 . 


x = 

- L 'xy 


j — — 


x — 

-Lyz ~ 


( V 2 + z 2 )p{x, y, z) dx dy dz 


(ar + z )p{x, y , z) dx dy dz 
( x 2 + y 2 )p(x, y , z) dx dy dz 

III Xyp ( X ’ Vl ^ dxdy dz 

J J J xz P( x ,y, z) dx dy dz 

III yzp ^ x,y,z ^ dxdydz - 


(8.23) 


If the body has uniform density, If, is determined exclusively by the shape of 
the rigid body (see Figure 8.5). 

Given an inertia matrix If,, the principal axes of inertia are given by 
the eigenvectors and eigenvalues of I b . Let V\,V 2 ,V 3 be the eigenvectors of 
lb and Ai, A 2 , A 3 be the corresponding eigenvalues. Then the principal axes 
of inertia are in the directions of ri,U 2 ,U 3 , and the scalar moments of inertia 
about these axes, the principal moments of inertia, are A!,A 2 ,A 3 > 0. One 
of the principal axes maximizes the moment of inertia among all axes passing 
through the center of mass, and one of the principal axes minimizes the moment 
of inertia. For bodies with symmetry, often the principal axes of inertia are 
apparent. The principal axes may not be unique, however; for a uniform-density 
solid sphere, for example, any three orthogonal axes intersecting at the center 
of mass is a set of principal axes, and the minimum principal moment of inertia 
is equal to the maximum principal moment of inertia. 

If the principal axes of inertia are aligned with the axes of {b}, the off- 
diagonal terms of If, are all zero, and the eigenvalues are the scalar moments of 
inertia I xx , I yy , and I zz about the x, y, and z axes, respectively. In this case, 
the equations of motion ( 8 . 22 ) simplify to 


m b 


T (I„ If/y jXyOJ z 

Tyyyly ' (I/-,; di zz ^CJ x UJ z 

f-'ZZ'Xz T (lyy I/:;/: *)X :r LGy 


(8.24) 


where uj b = (ui x ,u! y ,w z ). When possible, we choose the axes of {b} to be aligned 
with the principal axes of inertia, to reduce the number of nonzero entries in I b 
and to simplify the equations of motion. 

Examples of common uniform-density solid bodies, their principal axes of 
inertia, and the principal moments of inertia obtained by solving the inte¬ 
grals (8.23), are given in Figure 8.5. 
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rectangular parallelepiped 
volume = abc 
I xx = m{w 2 + h 2 )/ 12 
Xy V = m(£ 2 + h 2 )/12 
Zzz = m(£ 2 + w 2 )/12 


circular cylinder 
volume = nr 2 h 


X xx = m(3 r 2 + h 2 )/ 12 
X yy = m(3r 2 + h 2 )/ 12 
I zz = mr 2 /2 



volume = 4nabc/3 
X xx = m{b 2 + c 2 )/5 
l yy = m (a 2 + c 2 )/5 
Izz = m(a 2 + b 2 )/5 


Figure 8.5: The principal axes and the inertia about the principal axes for 
uniform density bodies of mass m. Note that the x and y principal axes of the 
cylinder are not unique. 


An inertia matrix If, can be expressed in a rotated frame {c} described by 
the rotation matrix Rb c . Denoting this inertia matrix as I c , and knowing that 
the kinetic energy of the rotating body must be identical regardless of the chosen 
frame, we have 



1 T 

2 W b XfrUJb 

^(-^bc^c) Ib(^bc^c) 
— W c (i? &c If,i?bc) w c- 


In other words, 

X c = RbcZbRbc- (8.25) 

If the axes of {b} are not aligned with the principal axes of inertia, then we can 
diagonalize the inertia matrix by expressing it instead in the rotated frame {c}, 
where the columns of Rb c correspond to eigenvalues of !{,. 

Sometimes it is convenient to represent the inertia matrix in a frame at a 
point not at the center of mass of the body, such as at a joint. Steiner’s 
theorem can be stated as follows. 


Theorem 8.2. The inertia matrix l q about a frame aligned with {b}, but at a 
point q = {q Xl qy,qz) T in {b}, is related to the inertia matrix lb calculated at 
the center of mass by 

l q =l b + m{q T qI 3 - qq T ), (8.26) 

where I is the 3x3 identity matrix and m is the mass of the body. 

Steiner’s theorem is a more general statement of the parallel-axis theorem, 
which states that the scalar inertia Id about an axis parallel to, but a distance 
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d from, an axis through the center of mass, is related to the scalar inertia I cm 
about the axis through the center of mass by 

Id = Icm+md 2 . (8.27) 

Proofs of these are left to the exercises. 

Equations (8.25) and (8.26) are useful for calculating the inertia of a rigid 
body consisting of multiple component rigid bodies. First calculate the inertia 
matrices of the n component bodies in terms of frames at their individual centers 
of mass. Then choose a common frame {common} and use Equations (8.25) and 
(8.26) to express each of the inertia matrices in this common frame. Once the 
individual inertia matrices are expressed in {common}, they can be summed to 
get the inertia matrix I comm on for the composite rigid body. 

In the case of motion confined to the x-y plane, where u>b = (0, 0, lo z ) t and 
the inertia of the body about the z-axis through the center of mass is given 
by the scalar l ZZl the spatial rotational dynamics (8.22) reduce to the planar 
rotational dynamics 

m z = 1 zz Cj z , 

and the rotational kinetic energy is 

K = 


8.2.2 Twist-Wrench Formulation 


The linear dynamics (8.21) and the rotational dynamics (8.22) can be written 
in the following combined form: 


m b 


' lb 

0 


LOb 

+ 

M 

0 


' i b 

0 


LO b 

. fb . 


0 

ml 


Vb 

0 

M . 


0 

mJ 


Vb 


(8.28) 


where I is the 3x3 identity matrix. With the benefit of hindsight, and also 
making use of the fact that [u]u = v x v = 0 and [u] T = — [u], we write Equa¬ 
tion (8.28) in the following equivalent form: 


m b 


' lb 

0 

LOb 

_l_ 

M 

M ' 


i b 

0 

LOb 

. fb . 


0 

ml 

Vb 

ir 

0 

M . 


0 

ml 

Vb 



' lb 

0 

LOb 


M 

0 

T 

' ib 

0 

LOb 



0 

ml 

Vb 


M 

M . 


0 

ml 

Vb 


Written this way, each of the terms can now be identified with six-dimensional 
spatial quantities as follows: 

(i) (uib, Vb) and ( mb , fb) can be respectively identified with the body twist 14 
and body wrench J-}, 


V b 


LO b 

Vb 


Tb 


m b 

fb 


(8.30) 
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(ii) The spatial inertia matrix Q b £ R 6x6 is defined as 

G b = 


lb o 

0 ml 


(8.31) 


where I denotes the 3x3 identity matrix. As an aside, the kinetic energy 
of the rigid body can be expressed in terms of the spatial inertia matrix 
as 

1 _ 1 1 _ 

(8.32) 


Kinetic Energy = \^l b bj h + ^nvu^ v b = ^V, T G b V b . 


(iii) The spatial momentum V b £ R 6 is defined as 
V b = 


l b iO b 


' lb 

0 


UJ b 

mv b 


0 

ml 


Vb 


= GbVb- 


(8.33) 


Observe that the V b term in Equation (8.29) is left-multiplied by the matrix 


M o 
M M 


(8.34) 


We now explain the origin and geometric significance of this matrix. First, 
recall that the cross product of two vectors u >\, u >2 £ R 3 can be calculated using 
skew-symmetric matrix notation as follows: 


[wi X W 2 ] = [wi]M - [W2][wi]- (8.35) 

The matrix in (8.34) can be thought of as a generalization of the cross-product 
operation to six-dimensional twists. Specifically, given two twists Vi = (wi,r>i) 
and V 2 = (u) 2 ,V 2 ), we perform a calculation analogous to (8.35): 


[wi] Vi 

[w 2 ] V 2 


[w 2 ] V2 

[wi] Vl 

0 0 

0 0 


0 0 

- 1 

O 

O 


[wi][w 2 ] - [w 2 ][wi] [ui]v 2 - [w 2 ]«l 


' [c J] v' ' 

0 0 


0 0 


which can be written more compactly in vector form as 


' w' ' 


' [wi] 

0 

0J2 

. v ' 


M 

[wi] _ 

y 2 


This generalization of the cross product to two twists Vi and V 2 is called the 
Lie bracket of Vi and V 2 . 


Definition 8.1. Given two twists Vi = (wi,ui) and V 2 = (w 2 ,t ! 2 ), the Lie 
bracket of Vi and V 2 , written either as [Vi, V 2 ] or adyj (V 2 ), is defined as follows: 


[Vi,V 2 ] 


\ [Wl] 

0 

w 2 

[ [«l] 

M . 

v 2 


advj (V 2 ) £ se( 3). 


(8.36) 
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Given V = ( w,v ), we further define the following notation for the 6x6 matrix 
representation [ady]: 


[ady] 


M o 
M M 


e R 


6x6 


(8.37) 


With this notation the Lie bracket [Vi, V 2 ] can also be expressed as 


[Vi, V 2 ] = ady 1 (V 2 ) = [adyJV 2 - 


(8.38) 


Definition 8.2. Given a twist V = ( ui,v ) and wrench T = (m,/), define the 
mapping 


ady (T) = [ad v ] T J- = 


\ M 

0 

T 

m 


-[w]m - [v\f 

[ M 

M . 


. /. 


-M / 


(8.39) 


Using the above notation and definitions, the dynamic equations for a single 
rigid body can now be written as 


T b = g b V h - ady fj (Vh) 

= o b v b — [&dv b ] T g b v b . 


(8.40) 


Note the analogy between Equation (8.40) and the moment equation for a ro¬ 
tating rigid body: 

m b = l b uj b - [uj b ] T l b uj b . (8-41) 

Equation (8.41) is simply the rotational component of (8.40). 


8.3 Newton-Euler Inverse Dynamics 

We now consider the inverse dynamics problem for an n-link open chain con¬ 
nected by one-degree-of-freedom joints. Given the joint positions 9 £ R", ve¬ 
locities 9 £ R n , and accelerations 9 £ R", the objective is to calculate the 
right-hand side of the dynamics equation 

t = M{6)9 + h(6,0). 

The main result is a recursive inverse dynamics algorithm consisting of a for¬ 
ward and backward iteration stage. In the forward iteration, the velocities and 
accelerations of each link are propagated from the base to the tip, while in 
the backward iteration, the forces and moments experienced by each link are 
propagated from the tip to the base. 

8.3.1 Derivation 

A body-fixed reference frame {*} is attached to the center of mass of each link 
i, i = 1, ..., n. The base frame is denoted {0}, and a frame at the end-effector 
is denoted {n + 1}. This frame is fixed in {n}. 
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When the manipulator is at the home position, with all joint variables zero, 
we denote the configuration of frame {j} in {*} as M it j £ SE( 3), and the 
configuration of {z} in the base frame {0} using the shorthand Mi = Mo,,. 
With these definitions, M, ; _i i? ; can be calculated as 

Mi_ hi = M-\M,. (8.42) 


The screw axis for joint i, expressed in the link frame {*}, is Ai. This same 
screw axis is expressed in the space frame {0} as Si, where the two are related 
by 

Ai = Ad Mr i(<Sj). 

Defining Tj j £ SE( 3) to be the configuration of frame {j} in {*} for arbitrary 
joint variables 9, then the configuration of {*} relative to {* — 1} given the joint 
variable 0i is 

T i _y i {e i )=M i _y i e^ 9i . 

We further define the following notation: 

(i) Denote the twist of link frame {*}, expressed in frame {*} coordinates, by 

Vi = (uJi,Vi). 


(ii) Denote the wrench transmitted through joint i to link frame {*}, expressed 
in frame {*} coordinates, by J 7 , = (rrii,fi). 

(iii) Let Si £ K 6x6 denote the spatial inertia matrix of link i, expressed relative 
to link frame {z}. Since we assume that all link frames are situated at the 
link center of mass, Si has the block-diagonal form 


Si 


x, o 

0 mil ’ 


(8.43) 


where Ii denotes the 3x3 rotational inertia matrix of link i and m,; is the 
link mass. 


With these definitions, we can recursively calculate the twist and acceleration 
of each link, moving from the base to the tip. The twist of link i, expressed in 
{z}, is the sum of the twist of link i — 1, Vj_i, but expressed in {«}, and the 
added twist due to the joint rate Op 

Vi = Ad + AA (8.44) 


The accelerations V,; can also be found recursively. Noting that 


[V,] = -T t -i4V t \Tr\. + Ti.^Vi]!^. + T^Vij-Tr 1 ,. + [A t ]9 t , 


dt 


j t T i-hi = M i -y i [Ai]e^ 6i e i = Mi_ 1>i e^ di [A i ]e i 


n — 1 


_ T~ 

dt < - 1 > < 


_ rri —1 rji rri —1 

i— 1,2"^ 2—1,2 5 


and 
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Joint 



Figure 8.6: Free body diagram illustrating the moments and forces exerted on 
link i. 


it can be shown that 


V, = AA + Adr^Vi-i) + [Adr^tVi-i), A^], 


(8.45) 


where Adr i _ 1 (Vj_i), AiOi denotes the Lie bracket of Ad^.i(Vi-i) with 

AiOi- Note that since [Ai,Ai] = 0 and Ad r . i _ 1 (Vj_i) = V, — A^Oi, we ob¬ 
tain the alternative but equivalent formula 


Vi = AA + Ad Tj 4 _j {Vi- 1) + [Vi, AA}. 


(8.46) 


Once we have determined all the link twists and accelerations moving out¬ 
ward from the base, we can calculate the joint torques or forces by moving 
inward from the tip. The rigid-body dynamics (8.40) tells us the total wrench 
that acts on link i given V,; and Vi, and the total wrench acting on link i is the 
sum of the wrench J~ t transmitted through joint i and the wrench applied to 
the link through joint i + 1 (or, for link n, the wrench applied to the link by the 
environment at the end-effector frame {n + 1}) expressed in the frame i : 

QiVi ~ ady. {Qi Vi) = A~ Ad£ j+M (7)+i). (8.47) 

See Figure 8.6. Solving from the tip toward the base, at each link we solve for 
the only unknown in Equation (8.47): A- Since joint i has only one degree 
of freedom, five dimensions of the six-vector J~ t are provided “for free” by the 
structure of the joint, and the actuator only has to provide the force or torque 
in the direction of the joint’s screw axis: 

Ti=TfAi. (8.48) 


These equations provide the torques required at each joint, solving the inverse 
dynamics problem. 
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8.3.2 Newton-Euler Inverse Dynamics Algorithm 

Initialization: Attach a frame {0} to the base, frames {1} to {n} to the 
centers of mass of links { 1 } to {n}, and a frame {n + 1 } at the end-effector, 
fixed in the frame {n}. Define to be the configuration of {*} in {* — 1} 

when 9% = 0. Let A t be the screw axis of joint i expressed in {i}, and Qi be 
the 6 x 6 spatial inertia matrix of link i. Define Vo to be the twist of the base 
frame {0} expressed in base frame coordinates. (This quantity is typically zero.) 
Let g 6 R 3 be the gravity vector expressed in base frame {0} coordinates, and 
define V 0 = (0,—g). (Gravity is treated as an acceleration of the base in the 
opposite direction.) Define fF n +\ = J^ip = (m t i p , /tip) to be the wrench applied 
applied to the environment by the end-effector expressed in the end-effector 
frame {n + 1 }. 


Forward iterations: Given 9 , 9 , 9 , for i = 1 to n do 



= 

. e [M6i 

(8.49) 

v, 

= Aclj\ 

i_i (Vj_i) + Ai9i 

(8.50) 

Vi 

= Aclj 1 . 

t _!(Vi-i) + [Vi,Ai]9i + A z 9i. 

(8.51) 

Backward iterations: 

For i = 

n to 1 do 


Ti = 

Ad ?, +1 , 

i{Fi+i) + GiVi — ady. (C/iVi) 

(8.52) 

Ti = 

TjA % . 


(8.53) 


8.4 Dynamic Equations in Closed Form 

In this section we show how the equations in the recursive inverse dynamics 
algorithm can be organized into a closed-form set of dynamics equations of the 
form t = M(9)9 + c(9 1 9) + g{9). 

Before doing so, we prove our earlier assertion that the total kinetic energy 
K, of the robot can be expressed as 1C = \9 T M(9)9. We do so by noting that /C 
can be expressed as the sum of the kinetic energies of each link: 

1 " 

^ = (8.54) 

Z i=1 

where V,; is the twist of link frame {*}, and Qi is the spatial inertia matrix 
of link i as defined by Equation (8.31) (both are expressed in link frame {i} 
coordinates). Let To,(6/ ,... ,9i) denote the forward kinematics from the base 
frame {0} to link frame {*}, and let Jib (6) denote the body Jacobian obtained 
from Toi. Note that Jib as defined is a 6 x i matrix; we turn it into a 6 x n 
matrix by filling in all entries of the last n — i columns with zeros. With this 
definition of J^, we can write 


Vi = Jib{9)9 , i = 1, ... ,n. 
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The kinetic energy can then be written 


1 




\ i— 1 


The term inside the parentheses is precisely the mass matrix M(0 ): 

n 


(8.55) 


(8.56) 


We now return to the original task of deriving a closed-form set of dynamic 
equations. We start by defining the following stacked vectors: 


V = 


T = 


Vi 

V n 

T i 

En 


p6n 


p 6n 


Further define the following matrices: 


A = 


Q = 


[ad v ] = 


... o 
... o 

' ’ ’ A n 

... o 
... 0 


[ad. 


■Ae J 


S(9) = 


A! 0 

0 a 2 

0 ••• 

Qi 0 

o g 2 

0 . Qn 

[ad Vl ] 0 

0 [ady 2 ] • • 

0 . 

0 

0 I ac WJ 

0 

0 0 
[Adr 21 ] 0 

0 [Adr 32 ] 


p6 nxn 


p6nx6n 


[ady n 


p6nx6n 


o 

o 

i &d Aj n 

0 

0 

0 


p6nx6n 


[Adr„,„_i] 0 


(8.57) 


(8.58) 


(8.59) 


(8.60) 


(8.61) 


(8.62) 


p6nx6n 


.(8.63) 
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We write S(9) to emphasize the dependence of S on 9. Finally, define the 
following stacked vectors: 


V b 


ase 


v b 


ase 


•^tip 


AdT lo (V 0 ) 

0 


0 

AdTi 0 (Vo) 

0 


])6n 


i>6n 


Ad T n+ i, n ™+l) 


p6 n 


(8.64) 


(8.65) 


( 8 . 66 ) 


Note that A £ R 6raxn and Q £ R 6 " x 6 " are constant block-diagonal matrices, 
in which A contains only the kinematic parameters, while Q contains only the 
mass and inertial parameters for each link. 

With the above definitions, our earlier recursive inverse dynamics algorithm 
can be assembled into the following set of matrix equations: 


V 

= S(9)V + A0 + V base 

(8.67) 

V 

= 5(0) 1> + AO - [ad^] (5(0) V + V base ) + Vbase 

(8.68) 

T 

= s T (o) t + gv- [ad v ] T gv + j- tip 

(8.69) 

T 

= A t t. 

(8.70) 


S(9) has the property that S n (6) = 0 (such a matrix is said to be nilpotent 
of order n), and one consequence verifiable through direct calculation is that 
(/ - 5(d ))" 1 =1 + S(9) + ...+ S n ~ 1 (9). Defining £(0) = (I - S(9))-\ it can 
further be verified via direct calculation that 



i 

0 

0 

... 0 " 


[Ad T21 ] 

/ 

0 

... 0 

m = 

[Ad rsi ] 

I Ad T 32 ] 

I 

... 0 


.. 

_ 1 

I Ad T„ 2 ] 

[ Ad T„ 3 ] 

... / 


We write C(9) to emphasize the dependence of C on 9. The earlier matrix 
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equations can now be reorganized as 

V = C{0) ^A9 + Vbase) (8.72) 

1> = C{0) (^+[ad^]5(0)V+[ad^]V base + Vba S e) (8.73) 

T = C T (9) (gv- [ad v ] T SV + Jiip) (8.74) 

r = A t F. (8.75) 

If the robot applies an external wrench at the end-effector, this can be 
included into the following dynamics equation, 

r = M(9)9 + c(0,0) + g(9) + J T (9)F ti p , (8.76) 

where J[0) denotes the Jacobian of the forward kinematics expressed in the 
same reference frame as T t \ v . and 

M{9) = A T C T (9)gC(9)A (8.77) 

c(M) = -A T c T {e)(gc{e)[ad Ad }s{e) + [ad v ] T g)c(9)Ae (8.78) 

9 ( 6 ) = A T C T ( 0 )gC( 6 )V hase . (8.79) 

8.5 Forward Dynamics of Open Chains 

The forward dynamics problem is to solve 

M(9)6 = r(t) - h(0, 9) - J t (6)F tip (8.80) 

for 0, given 0, 6 , r, and the wrench applied by the end-effector (if ap¬ 


plicable). The term h(6,0) can be computed by calling the inverse dynamics 
algorithm with 9 = 0 and Fty, = 0. The inertia matrix M ( 9) can be computed 
using Equation (8.56). An alternative is to use n calls of the inverse dynamics 
algorithm to build M{9) column by column. In each of the n calls, set 0 = 0, 
0 = 0, and Fti p = 0. In the first call, the column vector 9 is all zeros except 
for a 1 in the first row. In the second call, 9 is all zeros except for a 1 in the 
second row, and so on. The r vector returned by the zth call is the ith column 
of M(0), and after n calls the n x n matrix M(9) is constructed. 

With M(0), h(8, 0), and Ftip, we can use any efficient algorithm for solving 
Equation (8.80), an equation of the form M9 = b , for 9. 

The forward dynamics can be used to simulate the motion of the robot given 
its initial state, the joint forces/torques r(t), and an optional external wrench 
J-tip(t), for t € [0,tf]- First define the function ForwardDynamics returning the 
solution to Equation (8.80), i.e., 

9 = ForwardDynamics(9 , 9 , r, J^ip). 
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Defining the variables q± = 0, q 2 = 0, the second-order dynamics (8.80) can be 
converted to two first-order differential equations 

9i = 92 

q 2 = ForwardDynamics(qi,q 2 1 T,J r ti p ). 

The simplest method for numerically integrating a system of first-order differ¬ 
ential equations of the form q = f(q,t ), q € R", is the first-order Euler iteration 

q(t + St) = q(t) + Stf(q(t), t ), 

where the positive scalar St denotes the timestep. The Euler integration of the 
robot dynamics is thus 

qi(t + St) = qi(t) + q 2 (t)St 

q 2 (t + St) = q 2 {t) + ForwardDynamics(qi,q 2 ,T,J r ti p )St. 

Given a set of initial values for gi(0) = 0(0) and q 2 {0) = 0(0), the above 
equations can be iterated forward in time to numerically obtain the motion 
9(t) = gi (t). 

Euler Integration Algorithm for Forward Dynamics 

• Inputs: Initial conditions 0(0) and 0(0), input torques r(t) and wrenches 
at the end-effector Fti p (t) for t € [0, t/], and the number of integration 
steps N. 

• Initialization: Set timestep St = tf/N , 0[O] = 0(0), 0[O] = 0(0). 

• Iteration: For k = 0 to N — 1 do 

0[k\ = ForwardDynamics(0[k\,9[k\,T(kSt),J-ti p (kSt)) 

6[k + 1] = 9[k\ + d[k\6t 
9[k + 1] = 9[k\ + 9[k\6t 

• Output: Joint trajectory 9(kSt) = 9[k], 9{k5t) = 0[fc], k = 0,... ,N. 

The result of the numerical integration converges to the theoretical result 
as the number of integration steps N goes to infinity. Higher-order numerical 
integration schemes, such as fourth-order Runge-Kutta, can yield closer approx¬ 
imations with fewer computations than the simple first-order Euler method. 

8.6 Dynamics in Task Space Coordinates 

In this section we consider how the dynamic equations change under a trans¬ 
formation to coordinates of the end-effector frame (task space coordinates). To 
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keep things simple we consider a six-degree-of-freedom open chain with joint 
space dynamics 

r = M(9)6 + h{0 , 9), 9 £ R 6 , r e R 6 . (8.81) 

We also ignore for the time being any end-effector forces The twist V = 

(w, v) of the end-effector is related to the joint velocity 9 by 

V = J(9)9, (8.82) 

with the understanding that V and J(9) are always expressed in terms of the 
same reference frame. The time derivative V is then 

V = j(0)9 + J{9)9. (8.83) 

At configurations 9 where J{9) is invertible, we have 

9 = J _1 V (8.84) 

9 = J -1 V — J~ 1 jj~ 1 V. (8.85) 

Substituting for 9 and 9 in Equation (8.81) leads to 

r = M(9) (J _1 V - J- 1 jj- 1 v) + h(9, J” 1 V), (8.86) 

where J~ T denotes ( J~ l ) T = (J T )~ 1 . Pre-multiply both sides by J~ T to get 

J~ t t = J~ T MJ~ X V — J~ T MJ~ 1 jj~ 1 V 

+J- T h(9,J~ 1 V). 

Expressing J~ t t as the wrench J 7 , the above can be written 

T = A (9)V + r,(9,V), 

where 

A (9) = J^M^J- 1 
p(9,V) = J- T h(9,J- 1 V)-A(9)jj~ 1 V. 

These are the dynamic equations expressed in end-effector frame coordinates. 
If an external wrench J- is applied to the end-effector frame, then assuming 
zero actuator effort, the motion of the end-effector frame is governed by these 
equations. Note the dependence of A (9) and r](9,V) on 9. If 9 were replaced by 
its inverse kinematics solution 9 = then one would obtain a differential 

equation strictly in terms of the end-effector frame’s displacement X £ SE( 3) 
and twist V. In practice, since X is usually obtained by measuring 9 and 
substituting into the forward kinematics, it is preferable to leave the dependence 
on 9 explicit. 


(8.87) 

( 8 . 88 ) 

(8.89) 

(8.90) 
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8.7 Constrained Dynamics 

Now consider the case that the n-joint robot is subject to a set of k holonomic 
or nonholonomic Pfaffian velocity constraints of the form 

A(9)9 = 0, A(<9)£M fcxn . (8.91) 

(See Chapter 2.4 for an introduction to Pfaffian constraints.) Such constraints 
can come from loop-closure constraints. As an example, the motion of the end- 
effector of a robot arm opening a door is subject to k = 5 constraints due to the 
hinges of the door. As another example, a robot writing with a pen is subject 
to k = 1 constraint that keeps the height of the tip of the pen above the paper 
at zero. In any case, we assume that the constraints do no work on the robot, 
i.e., the generalized forces r con due to the constraints satisfy 



This assumption means that r con must be a linear combination of the columns 
of A t (9 ), i.e., r con = A r (6 )A for some A £ since these are the generalized 
forces that do no work when 9 is subject to the constraints (8.91): 

(. A T (9)\) T 9 = A t A(9)9 = 0 for all A £ R k . 

For the writing robot example, the assumption that the constraint is workless 
means that there is no friction between the pen and the paper. 

Adding the constraint forces A T {9) A to the equations of motion, we can 
write the n + k constrained equations of motion in the n + k unknowns {9, A} 
(forward dynamics) or the n + k unknowns {r, A} (inverse dynamics): 

r = M(O)9 + h{0,0) + A T (9) A (8.92) 

A(0)6 = 0, (8.93) 

where A is a set of Lagrange multipliers and A T (0) A are the forces the robot 
creates against the constraints. From these equations, it should be clear that 
the robot has n — k velocity freedoms and k “force freedoms”—the robot is free 
to create any generalized force of the form A r {9) A. (For the writing robot, this 
is not quite true; the robot can only apply pushing forces into the paper and 
table, not pulling forces.) 

Often it is convenient to reduce the n + k equations in n + k unknowns to n 
equations in n unknowns, without explicitly calculating the Lagrange multipliers 
A. To do this, we can solve for A in terms of the other quantities and plug our 
solution into Equation (8.92). Since the constraints are satisfied at all times, 
the time rate of change of the constraints satisfies 

A(9)9 + A(9)9 = 0 . (8.94) 

Assuming that M(0) and A{9) are full rank, we solve Equation (8.92) for 9 , plug 
in to Equation (8.94), and omit the dependencies on 9 and 9 for conciseness to 
get 


Ad + M _1 (t -h- A T A) = 0. 


(8.95) 
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Using Ad = — Ad, after some manipulation, we get 

A = (AM- 1 A t )- 1 (AM~ 1 (t -h)- Ad). (8.96) 

Plugging Equation (8.96) into Equation (8.92) and manipulating further, we get 

Pt = P(Md + h) (8.97) 

where 

P = I-A t {AM~ 1 A t )- 1 AM- 1 , (8.98) 

where / is the n x n identity matrix. The n x n matrix P{d) is rank n — k , and 
it maps the joint generalized forces r to P(d)r, projecting away the generalized 
force components against the constraints while retaining the generalized forces 
that do work on the robot. The complementary projection (/ — P(d)) maps r 
to (I — P(0))t , the joint forces that act on the constraints and do no work on 
the robot. 

Using P(d ), we can solve the inverse dynamics by evaluating the right-hand 
side of Equation (8.97). The solution P(d)r is a set of generalized joint forces 
that achieves the feasible components of the desired joint acceleration 6. To 
this solution we can add any constraint forces A T (d) A without changing the 
acceleration of the robot. 

We can rearrange Equation (8.97) to the form 

Pgd = PgM- 1 ^ -h), (8.99) 

where 

Pg = M~ 1 PM = 1- M~ 1 A t (AM~ 1 A t )- 1 A. (8.100) 

The rank n — k projection matrix Pg(d) £ R rax " projects away the components 
of an arbitrary joint acceleration d that violate the constraints, leaving only the 
components Pg(d)d that satisfy the constraints. To solve the forward dynamics, 
evaluate the right-hand side of Equation (8.99), and the resulting Pgd is the set 
of joint accelerations. 

In Chapter 11.4 we discuss the related topic of hybrid motion-force control, in 
which the goal at each instant is to simultaneously achieve a desired acceleration 
satisfying Ad = 0 (consisting oin — k motion freedoms) and a desired constraint 
force A T (X) (consisting of k force freedoms). In that chapter, we use the task- 
space dynamics to represent the task-space end-effector motions and forces more 
naturally. 

8.8 Robot Dynamics in the URDF 

As described in Chapter 4.2 and illustrated in the UR5 Universal Robot De¬ 
scription Format file, the inertial properties of link i are described in the URDF 
by the link elements mass, origin (the position and orientation of the center of 
mass frame relative to a frame attached at joint i), and inertia, which speci¬ 
fies the six elements of the symmetric rotational inertia matrix on or above the 
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diagonal. To fully write the robot’s dynamics, for joint i we need the element 
origin, specifying the position and orientation of link i’s joint frame relative to 
link i — l’s joint frame when 9i = 0, and the element axis, which specifies the 
axis of motion of joint i. It is left to the exercises to translate these elements to 
the quantities needed for the Newton-Euler inverse dynamics algorithm. 


8.9 Actuation, Gearing, and Friction 

Until now we have been assuming the existence of actuators that directly provide 
commanded forces and torques. In practice, there are many types of actuators 
(e.g., electric, hydraulic, and pneumatic) and mechanical power transformers 
(e.g., gearheads), and the actuators can be located at the joints themselves or 
remotely, with mechanical power transmitted by cables or timing belts. Each 
combination of these has its own characteristics that can play a significant role 
in the “extended dynamics” mapping the actual control inputs (e.g., the current 
requested of amplifiers connected to electric motors) to the motion of the robot. 

In this section we provide an introduction to some of the issues associated 
with one particular, and common, configuration: geared DC electric motors 
located at each joint. This is the configuration used in the Universal Robots 
UR5, for example. 

Figure 8.7 shows the electrical block diagram for a typical n-joint robot 
driven by DC electric motors. For concreteness, we assume each joint is revo¬ 
lute. A power supply converts the wall AC voltage to a DC voltage to power 
the amplifier associated with each motor. A control box takes user input, for 
example in the form of a desired trajectory, as well as position feedback from 
encoders located at each joint. Using the desired trajectory, a model of the 
robot’s dynamics, and any errors in the current robot state relative to the de¬ 
sired robot state, the controller calculates the torque required of each actuator. 
Since DC electric motors nominally provide a torque proportional to the current 
through the motor, this torque command is equivalent to a current command. 
Each motor amplifier then uses a current sensor (shown external to the amplifier 
in Figure 8.7, but actually internal to the amplifier) to continually adjust the 
voltage across the motor to try to achieve the requested current. 3 The motion 
of the motor is sensed by the motor encoder, and the position information is 
sent back to the controller. 

The commanded torque/current is typically updated in the neighborhood of 
1000 times per second (1 kHz), and the amplifier’s voltage control loop may be 
updated at a rate ten times that or more. 

Figure 8.8 is a conceptual representation of the motor and other components 
for a single axis. The motor has a single shaft extending from both ends of 
the motor: one end drives a rotary encoder, which measures the position of 
the joint, and the other end becomes the input to a gearhead. The gearhead 
increases the torque while reducing the speed, since most DC electric motors 

3 The voltage is typically a time-averaged voltage achieved by the duty cycle of a voltage 
rapidly switching between a maximum positive voltage and a maximum negative voltage. 
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encoder signals 



Figure 8.7: A block diagram of a typical n-joint robot. Bold lines correspond 
to high-power signals, while thin lines correspond to communication signals. 


current 


position 

feedback 



encoder motor 


gearhead bearing link /+1 


Figure 8.8: The outer cases of the encoder, motor, gearhead, and bearing are 
fixed in link i, while the gearhead output shaft supported by the bearing is fixed 
in link i + 1. 


with an appropriate power rating provide torques that are too low to be useful 
for robotics applications. The purpose of the bearing is to support the gearhead 
output, freely transmitting torques about the gearhead axis while isolating the 
gearhead (and motor) from wrench components due to link i + 1 in the other 
five directions. The outer cases of the encoder, motor, gearhead, and bearing 
are all fixed relative to each other and to link i. It is also common for the motor 
to have some kind of brake, not shown. 
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8.9.1 DC Motors and Gearing 

A DC motor consists of a stator and a rotor that rotates relative to the stator. 
DC electric motors create torque by sending current through windings in a 
magnetic field created by permanent magnets, where the magnets are attached 
to the stator and the windings are attached to the rotor, or vice-versa. A DC 
motor has multiple windings, some of which are energized and some of which 
are inactive at any given time. The windings that are energized are chosen as a 
function of the angle of the rotor relative to the stator. This “commutation” of 
the windings occurs mechanically using brushes (brushed motors) or electrically 
using control circuitry (brushless motors). Brushless motors have the advantage 
of no brush wear and higher continuous torque, since the windings are typically 
attached to the motor housing where the heat due to the resistance of the 
windings can be more easily dissipated. In our basic introduction to DC motor 
modeling, we do not distinguish between brushed and brushless motors. 

Figure 8.9 shows a brushed DC motor with an encoder and a gearhead. 

The torque r, measured in newton-meters (Nm), created by a DC motor is 
governed by the equation 

r = k t I, 

where /, measured in amps (A), is the current through the windings. The 
constant k t: measured in newton-meters per amp (Nm/A), is called the torque 
constant. The power dissipated as heat by the windings, measured in watts 
(W), is governed by 

Pheat = I 2 R, 

where R is the resistance of the windings in ohms (fl). To keep the motor 
windings from overheating, the continuous current flowing through the motor 
must be limited. Accordingly, in continuous operation, the motor torque must 
be kept below a continuous torque limit T con t determined by thermal properties 
of the motor. 

A simplified model of a DC motor, where all units are in the SI system, can 
be derived by equating the electrical power consumed by the motor P e i ec = IV 
in watts (W) with the mechanical power P mec h = tw (also in W) and other 
power produced by the motor, 


IV = tw + I 2 R + LI— + friction and other power, 
at 

where V is the voltage applied to the motor in volts (V), w is the angular speed 
of the motor in radians per second (1/s), and L is the inductance due to the 
windings in henries (H). The terms on the right-hand side are the mechanical 
power produced by the motor, the power lost to heating the windings due to the 
resistance of the wires, the power consumed or produced by energizing or de¬ 
energizing the inductance of the windings (since the energy stored in an inductor 
is \LI 2 , and power is the time derivative of energy), and power lost to friction 
in bearings, etc. Dropping this last term, replacing tw by ktlw , and dividing 
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Figure 8.9: (Top) A cutaway view of a Maxon brushed DC motor with an en¬ 
coder and gearhead. (Cutaway image courtesy of Maxon Precision Motors, Inc., 
maxonmotorusa.com.) The motor’s rotor consists of the windings, commutator 
ring, and shaft. Each of the several windings connects different segments of the 
commutator, and as the motor rotates, the two brushes slide over the commu¬ 
tator ring and make contact with different segments, sending current through 
one or more windings. One end of the motor shaft turns the encoder, and the 
other end is input to the gearhead. (Bottom) A simplified cross-section of the 
motor only, showing the stator (brushes, housing, and magnets) in dark gray 
and the rotor (windings, commutator, and shaft) in light gray. 


by I, we get the voltage equation 

V = ktw + IR + L^. (8.101) 

at 

Often Equation (8.101) is written with the electrical constant k e (with units 
of Vs) instead of the torque constant k t , but in SI units (Vs or Nm/A), the nu¬ 
merical value of the two is identical; they represent the same constant property 
of the motor. So we prefer to use k t . 

The voltage term ktw in Equation (8.101) is called the back electromotive 
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Figure 8.10: The operating region of a current- and voltage-limited DC electric 
motor, and its continuous operating region. 


force or back-emf for short, and it is what differentiates a motor from simply 
being a resistor and inductor in series. It also allows a motor, which we usually 
think of as converting electrical power to mechanical power, to be a generator, 
converting mechanical power to electrical power. If the motor’s electrical inputs 
are disconnected (so no current can flow) and the shaft is forced to turn by some 
external torque, you can measure the back-emf voltage k t w across the motor’s 
inputs. 

For simplicity in the rest of this section, we ignore the L dl/dt term. This as¬ 
sumption is exactly satisfied when the motor is operating at a constant current. 
With this assumption, Equation (8.101) can be rearranged to 



expressing the speed w as a linear function of r (with a slope of —R/k%) for a con¬ 
stant V. Now assume that the voltage across the motor is limited to the range 
[—I4iaxj Tkmax] and the current through the motor is limited to [—/ max , +d m ax], 
perhaps by the amplifier or power supply. Then the operating region of the mo¬ 
tor in the torque-speed plane is as shown in Figure 8.10. Note that the signs of r 
and w are opposite in the second and fourth quadrants of this plane, and there¬ 
fore the product tw is negative. When the motor operates in these quadrants, 
it is actually consuming mechanical power, not producing mechanical power. 
The motor is acting like a damper. 
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Focusing on the first quadrant (t > 0, w > 0 ,tw > 0), the bounding line 
segment of the operating region is called the speed-torque curve. The no- 
load speed wo = V max /k t at one end of the speed-torque curve is the speed 
the motor spins at when it is powered by V ma . x but providing no torque. In 
this operating condition, the back-emf k t w is equal to the applied voltage, so 
there is no voltage remaining to create current (or torque). The stall torque 
T s taii = ktV max /R at the other end of the speed-torque curve is achieved when 
the shaft is blocked from spinning, so there is no back-emf. 

Figure 8.10 also indicates the continuous operating region where |r| < r con t. 
The motor may be operated intermittently outside the continuous operating 
region, but extended operation outside the continuous operating region raises 
the possibility that the motor will overheat. 

The motor’s rated mechanical power is P ra t e d = TcontW con tj where u> con t is 
the speed on the speed-torque curve corresponding to r con t- Even if the motor’s 
rated power is sufficient for a particular application, the torque generated by a 
DC motor is typically too low to be useful. Gearing is therefore used to increase 
the torque while also decreasing the speed. For a gear ratio G, the output speed 
of the gearhead is 


w e 


— ^Cmotor j G. 


For an ideal gearhead, zero power is lost in the conversion, i.e., T mo torW mo tor = 
Tgear^gear, wll ich implies 

Tgear — GT mo t or . 


In practice, some mechanical power is lost due to friction and impact between 
gear teeth, bearings, etc., so 


— ryGr mo tor ? 


where 77 < 1 is the efficiency of the gearhead. 

Figure 8.11 shows the operating region of the motor from Figure 8.10 when 
the motor is geared by G = 2 (with 77 = 1). The achievable torque doubles, 
while the achievable speed shrinks by a factor of two. Since many DC motors 
are capable of no-load speeds of 10,000 rpm or more, robot joints often have 
gear ratios of 100 or more to achieve an appropriate compromise between speed 
and torque. 


8.9.2 Apparent Inertia 

The motor’s stator is attached to one link and the rotor is attached to another 
link, possibly through a gearhead. Therefore, when calculating the contribution 
of a motor to the masses and inertias of the links, the mass and inertia of the 
stator must be assigned to one link and the mass and inertia of the rotor must 
be assigned to the other link. 

Consider a stationary link 0 with the stator of the joint 1 gearmotor attached 
to it. The rotational speed of joint 1, the output of the gearhead, is 8. Therefore 


274 


Dynamics of Open Chains 



Figure 8.11: The original motor operating region, and the operating region with 
a gear ratio G = 2 showing the increased torque and decreased speed. 


the motor’s rotor rotates at GO. The kinetic energy of the rotor is therefore 

ic = \x mto ,{G0) 2 = i G^otor 0\ 

apparent inertia 

where I ro tor is the rotor’s scalar inertia about the rotation axis and G 2 I ro t or is 
the apparent inertia (often called the reflected inertia) of the rotor about 
the axis. In other words, if you were to grab link 1 and rotate it manually, the 
inertia contributed by the rotor would feel as if it were a factor G 2 larger than 
its actual inertia, due to the gearhead. 

While the inertia X ro tor is typically much less than the inertia I\\ n y of the 
rest of the link about the rotation axis, the apparent inertia G 2 I roto r may be 
on the order of, or even larger than, Iii n k- 

One consequence as the gear ratio becomes large is that the inertia seen by 
joint i becomes increasingly dominated by the apparent inertia of the rotor. In 
other words, the torque required of joint i becomes relatively more dependent 
on Oi than on other joint accelerations, i.e., the robot’s mass matrix becomes 
more diagonal. In the limit when the mass matrix has negligible off-diagonal 
components (and in the absence of gravity), the dynamics of the robot are 
decoupled- the dynamics at one joint have no dependence on the configuration 
or motion of other joints. 

As an example, consider the 2R arm of Figure 8.1 with Li = L 2 = mi = 
m 2 = 1. Now assume that each of joint 1 and joint 2 has a motor of mass 1, 
with a stator of inertia 0.005 and a rotor of inertia 0.00125, and a gear ratio G 
(with 77 = 1 ). With a gear ratio G = 10, the mass matrix is 


M(0 ) 


4.13 + 2 cos 02 1.01 +cos 02 
1.01 + cos 0 2 1.13 
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(a) (b) 


Figure 8.12 


With a gear ratio G = 100, the mass matrix is 


M(9) 


16.5+ 2 cos 0 2 1.13 +cos 0 2 
1.13 +cos 0 2 13.5 


The off-diagonal components are relatively less important for the second robot. 
The available joint torques of the second robot are ten times that of the first 
robot, so despite the increases in the mass matrix, the second robot is capable 
of significantly higher accelerations and end-effector payloads. The top speed 
of each joint of the second robot is ten times less than that of the first robot, 
however. 

If the apparent inertia of the rotor is non-negligible relative to the inertia 
of the rest of the link, the Newton-Euler inverse dynamics algorithm must be 
modified to account for it. One approach is to treat the link as consisting of 
two separate bodies, the geared rotor driving the link and the rest of the link, 
each with its own center of mass and inertial properties (where the link includes 
the mass and inertia of the stator of any motor mounted to it); calculate the 
twist and acceleration of each during the forward iterations, accounting for the 
gearhead in calculating the rotor’s motion; calculate the wrench on the link as 
in Equation (8.52) and the wrench on the rotor during the backward iterations; 
express the rotor wrench in the link’s frame and sum the wrenches to get the 
total wrench J~, on the link; and project the total wrench to the motor axis 
as in Equation (8.53) to calculate the required motor torque r. The current 
command to the DC motor is then / com = T/(r)ktG). 

Taking into account apparent inertias, the recursive Newton-Euler inverse 
dynamics algorithm can be modified as follows: 


Initialization: Attach a frame {0 }l to the base, frames {1}l to {u}l to the 
centers of mass of links {1} to {n}, and frames {1 }_r to { n to the centers of 
mass of rotors {1} to {n} as in Figure 8.12. Frame {n + 1 }l is attached to the 
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end-effector and assumed fixed with respect to frame {n}n- M(i-i) L ,i L the 
configuration of {*}l relative to {i — 1}l when Qt = 0. At is the screw axis of 
joint i expressed in {i}l- Similarly, Si is the screw axis of joint i expressed in 
{i}n- Gi L is the 6x6 spatial inertia matrix of link i that includes the inertia of 
the attached stator, and Q iR is the 6x6 spatial inertia matrix of rotor i. G; is 
the gear ratio of motor i. Lastly, Vq l , Vq l and J 7 ( n +i) L are defined in the same 
way as Vo, V 0 and T n+ \ in Section 8.3.2. 


Forward iterations: Given 9 , 9, 9 , for i = 1 to n do 

T(i-i) L , iL = M (i _ 1)Lii J A ^ (8.102) 

V iL = Adr lj . i{4 _ 1 j J .(V (i _ 1)i )+M (8.103) 

Vi L = Ad T^^.^iVa-DJ + lVi^A^ + AA (8.104) 

V lR = S,G,A (8.105) 

V lR = StGiOi. (8.106) 

Backward iterations: For i = n to 1 do 

J= lL = Ad^ ( . +i) ^ ^ (J"( i+1)i ) + Gi L Vi L — ady.^ {Gi L V iL ) (8.107) 

Fi* = 0 iR V iR (8.108) 

n = T^A + G^Si. (8.109) 


Note that all of the above discussion neglects the mass and inertia due to 
the gearhead itself. If there is no gearing, then no modification to the original 
Newton-Euler inverse dynamics algorithm is necessary; the stator is attached to 
one link and the rotor is attached to another link. Robots constructed with a 
motor at each axis and no gearheads are sometimes called direct-drive robots. 

8.9.3 Friction 

The Lagrangian and Newton-Euler dynamics do not account for friction at the 
joints, but friction forces and torques in gearheads and bearings may be signif¬ 
icant. Friction is a complex phenomenon which is the subject of considerable 
current research, and any friction model is a gross attempt to capture average 
behavior of the micromechanics of contact. 

Friction models often include a static friction term and a velocity-dependent 
viscous friction term. The static friction term means that nonzero torque is 
required to cause the joint to begin to move. The viscous friction term indicates 
that the amount of friction torque increases with increasing velocity of the joint. 
See Figure 8.13 for some examples of velocity-dependent friction. 

Other factors may contribute to the friction at a joint, including the loading 
of the joint bearings, the time the joint has been at rest, temperature, etc. The 
friction in a gearhead often increases as the gear ratio G increases. 
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T fric 

T fric 

Tic 

Tic 

Tic 

Tic 






0 

0 

0 

0 

0 

0 





(a) (b) (c) (d) (e) (f) 


Figure 8.13: Examples of velocity-dependent friction models, (a) Viscous fric¬ 
tion, Tfric = b viscous 0. (b) Coulomb friction, Tf ric = Static sgn(0). Tf ric can 
take any value in [—& s t a tic> ^static] a t zer0 velocity, (c) Static plus viscous fric¬ 
tion, rf r i c = Static sgn(0) + 6 v iscous^- (d) Static and kinetic friction, requiring 
Tfric > |& s tatic| to initiate motion, and then Tf r i c = ^kinetic sgn(0) during motion, 
where 6 sta tic > ^kinetic- (e) Static, kinetic, and viscous friction, (f) A friction 
law exhibiting the Stribeck effect—at low velocities, friction decreases as velocity 
increases. 


8.9.4 Joint and Link Flexibility 

In practice, a robot’s joints and links are likely to exhibit some flexibility. For 
example, the flexspline element of a harmonic drive gearhead achieves essentially 
zero backlash by being somewhat flexible. A model of a joint with harmonic 
drive gearing, then, could include a relatively stiff torsional spring between the 
motor’s rotor and the link the gearhead attaches to. 

Similarly, links themselves are not infinitely stiff. The finite stiffness is ex¬ 
hibited as vibrations along the link. 

Flexible joints and links introduce extra states to the dynamics of the robot, 
significantly complicating the dynamics and control. While many robots are 
designed to be stiff to try to minimize these complexities, in some cases this is 
impractical due to the extra link mass required to create the stiffness. 

8.10 Summary 

• Given a set of generalized coordinates 9 and generalized forces t, the 
Euler-Lagrange equations can be written 

_ d^dC _ dL 

T ~ dt de do' 

where C{9,9) = K.(0,6) — V(9), K. is the kinetic energy of the robot, and 
V is the potential energy of the robot. 

• The equations of motion of a robot can be written in the following equiv- 
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alent forms: 


r = M{9)9 + h(9, 9) 

= M{8)8 + c(9,9)+g(8) 

= M(6)6 + 6 T T(0)6 + g(e) 

= M{9)8 + C{8,9)8 + g(9), 

where M ( 9 ) is the nxn symmetric positive-definite mass matrix, h(9 , 9) is 
the sum of generalized forces due to gravity and quadratic velocity terms, 
c(9,9) are quadratic velocity forces, g{9) are gravitational forces, T(d) is 
an n x n x n matrix of Christoffel symbols of the first kind obtained from 
partial derivatives of M{9) with respect to 9, and C(9,9) is the nxn 
Coriolis matrix whose (i, j) entry is given by 

n 

c ij (9,9) = J2 T Hk(0)dk. 

fc =i 


If the end-effector of the robot is applying a wrench Idp to the environ¬ 
ment, the term ,J T (9)X t i P should be added to the right-hand side of the 
robot’s dynamic equations. 




The symmetric positive-definite rotational inertia matrix of a rigid body 
is 


lb 


XXX 

-‘-'xx - L 'xy - L 'xz 

7 7 7 

-L'xy -L'yy yz j 

7 7 7 

-L'XZ -L'yz -L'ZZ 


where 


%cx = J B (y 2 + z 2 )p{X, y, z)dV I yy = f B (x 2 + z 2 )p(x, y, z)dV 
Izz = f B (x 2 + y 2 )p(X, y, z)dV X X y = - f B xyp(x, y, z)dV 
Ixz =~f B xzp(x, y, z)dV X yz = - f B yzp(x, y, z)dV, 


B is the body, dV is a differential volume element, and p(x,y : z) is the 
density function. 

• If I;, is defined in a frame {b} at the center of mass, with axes aligned 
with the principal axes of inertia, then If, is diagonal. 

• If {b} is at the center of mass but its axes are not aligned with the principal 
axes of inertia, there always exists a rotated frame {c} defined by the 
rotation matrix Rb c such that X c = R^ c XbRbc is diagonal. 

• If If, is defined in a frame {b} at the center of mass, then I 9 , the inertia 
in a frame {q} aligned with {b}, but displaced from the origin of {b} by 
q € R 3 in {b} coordinates, is 

X q =X b + m(q T qI 3 ~qq T ). 
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• The spatial inertia matrix Qb expressed in a frame {b} at the center of 
mass is defined as the 6x6 matrix 


G b 


lb 0 
0 ml 


• The Lie bracket of two twists Vi and V 2 is 


[Vi,V 2 ]=adv 1 (V 2 ) = [adv 1 ]V 2) 


where 


[ad v ] 


M 0 
M N 


G R 


6x6 


• The twist-wrench formulation of the rigid-body dynamics of a single rigid 
body is 

lb = GtVb — [advj T GbVb- 

• The kinetic energy of a rigid body is ^V^GbVb, and the kinetic energy of 
an open-chain robot is \0 T M{6)8. 

• The forward-backward Newton-Euler inverse dynamics algorithm is the 
following: 

Forward iterations: Given 8,6,8, for i = 1 to n do 

T,_ m = M z _ hi e^ 

Vi = Ad T^.AVi-i) + AA 

V, = Adr^^Vi-rJ + tVi.^fli+M- 


Backward iterations: For i = n to 1 do 

1, = Ady. +i .(J r j +1 ) + GiVi — ady.(GiVi) 

n = ijAi. 

• Let Jib{8) to be the Jacobian relating 8 to the body twist V,, in link i’s 
center-of-mass frame {*}. Then the mass matrix M(8) of the manipulator 
can be expressed as 


M(8) = J2&)GiM8). 

• The forward dynamics problem is to solve 

M(8)8 = r(t) - h(8,8) - J T (6)l tip 
for 8, using any efficient solver of equations of the form Ax = b. 






280 


Dynamics of Open Chains 


• The robot’s dynamics M(9)9 + h(9 1 9) can be expressed in the task space 
as 

T = A(9)V + r,(9,V), 

where T is the wrench applied to the end-effector, V is the twist of the end- 
effector, and J 7 , V, and the Jacobian J{9) are all defined in the same frame. 
The task-space mass matrix A (9) and gravity and quadratic velocity forces 
rj(9, V) are 

A(0) = J~ t M{9)J- 1 

v (e,v) = j- T h{e,j- 1 v)-A(e)jj~ 1 v. 

• Define two n x n projection matrices of rank n — k 

P{9) = I- A t (AM- 1 A t )- 1 AM~ 1 
P§(9) = M~ 1 PM = I- M~ 1 A t {AM~ 1 A t )- 1 A 

corresponding to the k Pfaffian constraints acting on the robot, A(9)9 = 0, 
A G R fexra . Then the n + k constrained equations of motion 

r = M(9)9 + h(9,9) + A T (9)\ 

A(0)6 = 0 

can be reduced to these equivalent forms by eliminating the Lagrange 
multipliers A: 


Pt = P(M9 + h ) 

Pg9 = PgM-\r-h). 

The matrix P projects away joint force/torque components that act on the 
constraints without doing work on the robot, and the matrix Px projects 
away acceleration components that do not satisfy the constraints. 

• An ideal gearhead (100% efficient) with a gear ratio G multiplies the torque 
at the output of a motor by a factor G and divides the speed by a factor 
G, leaving the mechanical power unchanged. The inertia of the motor’s 
rotor about its axis of rotation, as apparent at the output of the gearhead, 
is G 2 I ro tor. 


8.11 Software 


adV = ad(V) 

Computes [ady]. 

taulist = InverseDynamics(thetalist.dthetalist,ddthetalist,g,Ftip, 
Mlist,Glist,Slist) 
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Uses Newton-Euler inverse dynamics to compute the n-vector r of required joint 
forces/torques given 9, 9, 9, 0 , ^tip, a list of transforms Mi_specifying the 
configuration of the center-of-mass frame of link {z} relative to {i — 1 } when the 
robot is at its home position, a list of link spatial inertia matrices Qi , and a list 
of joint screw axes S t expressed in the base frame. 

M = MassMatrix(thetalist,Mlist,Glist,Slist) 

Computes the mass matrix M(9) given the joint configuration 9 , a list of trans¬ 
forms Mi-ij, a list of link spatial inertia matrices Qi, and a list of joint screw 
axes Si expressed in the base frame. 

c = VelQuadraticForces(thetalist,dthetalist,Mlist,Glist,Slist) 

Computes c{9, 9) given the joint configuration 9, joint velocities 9, a list of 
transforms Mi- iy, a list of link spatial inertia matrices Qi, and a list of joint 
screw axes Si expressed in the base frame. 

grav = GravityForces(thetalist,g,Mlist,Glist,Slist) 

Computes g(9) given the joint configuration 9, the gravity vector 0 , a list of 
transforms Mi_n, a list of link spatial inertia matrices Qi, and a list of joint 
screw axes S, expressed in the base frame. 

JTFtip = EndEffectorForces(thetalist,Ftip,Mlist,Glist,Slist) 

Computes J T (9)J-’ti p given the joint configuration 9, the wrench .7-tip applied by 
the end-effector, a list of transforms Mj_i i j, a list of link spatial inertia matrices 
Qi, and a list of joint screw axes Si expressed in the base frame. 

ddthetalist = ForwardDynamics(thetalist,dthetalist,taulist,g,Ftip, 
Mlist,Glist,Slist) 

Computes 9 given the joint configuration 9, joint velocities 9, joint forces/torques 
t, the gravity vector 0 , the wrench _Fti p applied by the end-effector, a list of 
transforms M,j_i i, a list of link spatial inertia matrices Qi, and a list of joint 
screw axes Si expressed in the base frame. 

[thetalistNext,dthetalistNext] = EulerStep(thetalist,dthetalist, 
ddthetalist,dt) 

Computes a first-order Euler approximation to {9(t+St),9(t+St)} given the joint 
configuration 9{t), joint velocities 9(t), joint accelerations 9(t), and a timestep 
St. 

taumat = InverseDynamicsTrajectory(thetamat,dthetamat,ddthetamat, 
g,Ftipmat,Mlist,Glist,Slist) 

The variable thetamat is an N x n matrix of robot joint variables 9, where the 
zth row corresponds to the n-vector of joint variables 9(t) at time t = (i— 1 )St, 
where St is the timestep. The variables dthetamat, ddthetamat, and Ftipmat 
similarly represent 9, 9, and J-tip as a function of time. Other inputs include the 
gravity vector 0 , a list of transforms AU-i.;, a list of link spatial inertia matrices 
Qi, and a list of joint screw axes S.; expressed in the base frame. This function 
computes an N x n matrix taumat representing the joint forces/torques r(t) 


282 


Dynamics of Open Chains 


required to generate the trajectory specified by 0(f) and Tti p (t). Note that it is 
not necessary to specify St. The velocities 0(f) and accelerations 0(f) should be 
consistent with 0(f). 

[thetamat.dthetamat] = ForwardDynamicsTrajectory(thetalist, 
dthetalist,taumat,g,Ftipmat,Mlist,Glist,Slist,dt,intRes) 

This function numerically integrates the robot’s equations of motion using Euler 
integration. The outputs are N x n matrices thetamat and dthetamat, where 
the ith rows correspond to the n-vectors 6((i — l)<5t) and 9{{i — 1 )St). Inputs 
are the initial state 0(0) and 0(0), an N x n matrix of joint forces/torques r(t), 
the gravity vector g, an IV x n matrix of end-effector wrenches a list 

of transforms Mi -a list of link spatial inertia matrices Gi, a list of joint 
screw axes S, : expressed in the base frame, the timestep St, and the number of 
integration steps to take during each timestep (a positive integer). 

8.12 Notes and References 
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8.13 Exercises 


1. Center of mass and link inertias for some standard shapes. 

2. Given a linear mapping L : U V between two vector spaces U and V, 
a choice of basis for U and V then leads to a particular matrix representation 
for L ; different bases for U and V then lead to different matrix representations 
for L. It is also possible to think of the spatial inertia matrix as a coordinate 
representation for a rigid body’s kinetic energy, in the following sense. Given 
a body-fixed reference frame {6} attached to the rigid body, let 14 denote the 
rigid body’s body twist, and G b G R 6x6 be the spatial inertia matrix expressed 
relative to {6}. The kinetic energy, recall, is then given by 

1 T 

Kinetic Energy = -V b G b V b . 

Now, given a different choice of body-fixed reference frame, say {a}, denote 
by V a and G a € R 6x6 the twist and spatial inertia matrix expressed relative 
to frame {6}, respectively. Since the kinetic energy is a physical invariant and 
should not depend on the choice of reference frame coordinates, it follows that 

\v^G b V b . = \v T a G a V a . 

It is therefore customary to express kinetic energy in the following reference 
frame-invariant way: 

Kinetic energy = (V, V) = ^ V T GV, 

with the understanding that both V and G are always expressed with respect 
to the same body-fixed reference frame. 

(a) Determine the appropriate transformation rule between G a and G b such 
that the kinetic energy as defined above is reference frame-invariant. 

(b) Given a body-fixed reference frame {6}, Recall that the spatial momentum 
expressed in frame {6}, denoted V b , was defined as 

V b = G b V b . 

Suppose one wishes to express the spatial momentum in terms of another body- 
fixed reference fame {a}. Determine the appropriate transformation rule be¬ 
tween V b and V a . 

(c) The dynamic equations for a single rigid body expressed in terms of a body- 
fixed reference frame {6} are of the form 

T h = G b V b - [ad v J T n- 

Given a difference choice of body-fixed reference frame, say {a}, such that T ab 
is constant, show that the corresponding dynamic equation expressed relative 
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to the {a} frame is given by 

Ta = G a v a - [ad v J T lP a . 

As a result the rigid body dynamic equations can be expressed in the general 
form 

I = GV - [ad v ] T V, 
without explicit reference to a body-fixed frame. 

Solution: (a) Recalling that V a = AdT ab (V b ), and writing the kinetic energy in 
terms of V a as \V^G a V ai it then follows that 

V b T G b V b = V^GaVa 

= V b T [Ad Tab ] T G a [Ad T JV b 

where 

l AdT J = ( [p ab ]R ab R ab ) ' 

From the above it follows that 

G b = [AdT ab ] T G a [Adx ab ] 

G a = [Ad Tb f G b [Ad Tba }. 

(b) Recalling also that the kinetic energy can also be expressed in terms of the 
spatial momentum of the moving rigid body, i.e., as ^ P b V bl The transformation 
rule for P b now follows from the invariance of the kinetic energy: if {a} denotes a 
second body-fixed frame as before, then clearly P b V b = P b AdT ba (V a ), implying 
that P a and P b must be related by 

P a = Ad* Tba (P b ). 

(The above also follows from G a V a = [Ad Tba ] T G b [Ad Tba ] [Ad Tab ] V b = [Ad Tba ] T G b V b .) 
The coordinate transformation rule for P b is thus given by the transpose of the 
adjoint map Ad T , which is the same as for the wrench P b . 


3. Show that the time derivative of the mass matrix, M(8), can be explicitly 
written 


M = A t jC t T t [ad Ad ] T jC T gCA + A T C T gC\ad A g]VA. 


4. In the dynamic equations for an open chain, i.e., 

r = M(6)9 + c(6,6) + n(8), 
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Figure 8.14: * 


c{9,9) is often referred to as the Corlios terms. Earlier in Equation (8.78) we 
showed that c(9,9) can expressed as c{9,9 ) = A{9,9)9, where 

A(0,9) = A t C t (GC [ad^jr - [ad v fg) LA £ R nx ". (8.110) 

In the Lagrangian formulation we further showed that the Coriolis terms can 
be expressed as c(9,9) = C{9,9)9, where the ( k,j ) entry of C{9,9) £ R” xrl is 
given by 

n 

c kJ = r ijk9i, 

i=l 

where the T^k are the Christoffcl symbols of the first kind associated with the 
mass matrix M{9) = (niy) £ K nxrl . It turns out that the matrix M — 2 C with 
C defined as above is skew-symmetric; this property turns out to be important 
in proving the stability of certain dynamic model-based control laws (our later 
chapter on robot control also relies upon this result). 

(a) Show that M — 2 C is skew-symmetric. 

(b) Show that M — 2A with A defined as above is not skew-symmetric. 

(c) {See Ploen thesis 

5. Efficient recursive formulas for evaluating the mass matrix. 

6 . 

7 . Assuming the joint space dynamics are of the form 

t = M{9)9 + C(9,9) + N(9), (8.111) 

and the differential forward kinematics is of the form V = J{9)9 , where V £ 
se(3) denotes the velocity of the tip frame, the corresponding dynamics in SE{ 3) 
assume the form 

F = MV + C + IV, (8.112) 

where F = J~ t t £ se(3)* corresponds to the moment-force pair at the tip, and 


M = 

(.JM 1 

(8.113) 

C = 

J ~ TM (p-') v + J - Tc 

(8.114) 

N = 

J~ t N. 

(8.115) 


The above can be verified by noting that V = J9 + J9 , and replacing 9 in the 
original dynamics equations by 9 = J~ 1 V + 
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The physical meaning that can be associated with these equations is that, 
assuming zero joint torques, and given an external input force F(t) £ se(3)* 
applied at the tip frame, the motion of the tip frame is then governed by (8.112). 
Note the dependence of M{9 ), C(0,0), and N on 9 and 9\ if 9 and 9 in (8.112) 
are replaced by their inverse kinematics solutions 9 = f~ 1 (X) and 9 = J~ 1 (9)V, 
then a differential equation strictly in terms of the tip frame parameters X £ 
SE( 3) and V £ se(3) can be obtained. In practice, however, integrating these 
equations is cumbersome; to obtain the resulting tip frame trajectory X(t), it is 
far more preferable to integrate J T F = M(9)9 + C(9 , 9) + N(9) to obtain 0(t), 
and to substitute this into the forward kinematics relation to obtain X(t) = 

nm)- 

8 . 

The 2R open chain robot of Figure 8.15(a) is shown in its zero configuration. 
Assuming the mass of each link is concentrated at the tip and neglecting its 
thickness, the robot can be modeled as shown in Figure 8.15(b). Assume mi = 
m 2 = 2, Li = L 2 = 1, g = 10 m/sec, and the link inertias I\ and I 2 (expressed 
in their respective link body frames {6}i and {6} 2 ) are 


' 0 

0 

0 ' 
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0 

0 ' 
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0 
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Assume the total potential energy of the robot at its zero configuration is zero. 

(a) For arbitrary 9\, 9 2 , 9\, 0 2 , 9 1 , 9 2 , determine the body twists I4 1 and I4 2 of 
the link frames { 61 } and {b 2 }. 

(b) For arbitrary 0\, 9 2 , 6\, 0 2 , 6i, 9 2 , determine the kinetic energies and 
Ek 2 of the two links, and the total potential energy E p of the robot. 

(c) Use the Lagrangian method to derive the dynamic equations, and determine 
the input torques t\ and t 2 when 9\ = 9 2 = 7t/4 and 9\ = 9 2 = 9\ = 9 2 = 0. 

(d) Determine the mass matrix M(6) when 9 1 = 9 2 = tt/ 4. 

Solution: 

See figure. 

9 . Intuitively explain the shapes of the end-effector force ellipsoids in Figure 8.4 
based on the the point masses and the Jacobians. 

10 . Prove the parallel axis theorem, and then prove Steiner’s theorem. If it is 
helpful, you may assume the rigid body consists of point masses and that the 
axes of {b} are aligned with the principal axes of inertia of the body, but then 
you should show that these assumptions are not needed. 


11. Provide a general expression for the rotational inertia matrix I c in a frame 
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Figure 8.15: 2R open chain. 


{c} in terms of Tt c = ( RbaP ) and expressed in a center-of-mass frame {b}. 

12. Consider a cast iron dumbbell consisting of a cylinder connecting two solid 
spheres at either end of the cylinder. The density of the dumbbell is 7500 kg/m 3 . 
The cylinder has a diameter of 4 cm and a length of 20 cm. Each sphere has 
a diameter of 20 cm. (a) Find the approximate rotational inertia matrix If, in 
a frame {b} at the center of mass with axes aligned with the principal axes of 
inertia of the dumbbell, (b) Write the spatial inertia matrix Qt,. 

13. Program a function to efficiently calculate h(9,9) = c(9,9) + g(9) using 
Newton-Euler inverse dynamics. 

14. (a) Conceptually develop a computationally efficient algorithm for de¬ 
termining the mass matrix M(9 ) using Equation (8.56). (b) Implement this 
algorithm. 

15. The function InverseDynamicsTrajectory requires the user to enter not 
only a time sequence of joint variables thetamat, but also a time sequence of 
joint velocities dthetamat and accelerations ddthetamat. Instead, the func¬ 
tion could use numerical differencing to approximately find the joint velocities 
and accelerations at each timestep, using only thetamat. Write an alternative 
InverseDynamicsTrajectory function that does not require the user to enter 
dthetamat and ddthetamat. Verify that it yields similar results. 

16. Give the steps that rearrange Equation (8.97) to get Equation (8.99). 
Remember that P(9) is not full rank and cannot be inverted. 

17. Give the equations that would convert the joint and link descriptions in a 
robot’s URDF file to the data Mlist, Glist, and Slist, suitable for using with 


















288 


Dynamics of Open Chains 


the Newton-Euler algorithm InverseDynamicsTrajectory. 


18 . Consider a motor with rotor inertia X r otor connected through a gearhead of 
gear ratio G to a load with a scalar inertia T\\ n y about the rotation axis. The 
load and motor are said to be inertia matched if, for any given torque r m at 
the motor, the acceleration of the load is maximized. The acceleration of the 
load can be written 

;; Gr m 




link 


G 2 X r , 


Solve for the inertia matching gear ratio i/liink/Zrotor by solving d9/cLG = 0. 
Show your work. 




Chapter 9 


Trajectory Generation 


During robot motion, the robot controller is provided with a steady stream of 
goal positions and velocities to track. This specification of the robot position 
as a function of time is called a trajectory. In some cases, the trajectory is 
completely specified by the task -for example, the end-effector may be required 
to track a known moving object. In other cases, as when the task is simply to 
move from one position to another in a given time, we have freedom to design the 
trajectory to meet these constraints. This is the domain of trajectory planning. 
The trajectory should be a sufficiently smooth function of time, and it should 
respect any given limits on joint velocities, accelerations, or torques. 

In this chapter we consider a trajectory as the combination of a path , a 
purely geometric description of the sequence of configurations achieved by the 
robot, and a time scaling , which specifies the times when those configurations 
are reached. We consider three cases: point-to-point straight-line trajectories 
in both joint space and task space; trajectories passing through a sequence of 
timed via points; and minimum-time trajectories along specified paths. Finding 
paths that avoid obstacles is left to Chapter 10. 

9.1 Definitions 

A path 9(s) maps a scalar path parameter s, assumed to be zero at the start 
of the path and one at the end, to a point in the robot’s configuration space 
0, 9 : [0,1] —> 0. As s increases from 0 to 1, the robot moves along the path. 
Sometimes s is taken to be time, and is allowed to vary from time s = 0 to 
the total motion time s = T, but it is often useful to separate the role of the 
geometric path parameter s from the time parameter t. A time scaling s(t) 
assigns a value s to each time t € [0,T], s : [0, T] —> [0,1]. 

Together a path and time scaling define a trajectory 9(s(t)), or 6(t) for short. 
Using the chain rule, the velocity and acceleration along the trajectory can be 
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written as 


o de - 


d 2 e _ 

ds 2 


(9.1) 

(9.2) 


To ensure that the robot’s acceleration (and therefore dynamics) are well de¬ 
fined, each of 9(s) and s(t) must be twice differentiable. 


9.2 Point-to-Point Trajectories 

The simplest type of motion is from rest at one configuration to rest at another. 
We call this a point-to-point motion. The simplest type of path for point-to- 
point motion is a straight line. Straight-line paths and their time scalings are 
discussed below. 

9.2.1 Straight-Line Paths 

A “straight line” from a start configuration 0 sta rt to an end configuration 0 en( j 
could be defined in joint space or in task space. The advantage of a straight-line 
path from # sta rt to 0 e nd in joint space is simplicity: since joint limits typically 
take the form 6^ m i n < 9i < 0i jlnax for each joint i, the allowable joint configu¬ 
rations form a convex set 0f ree in joint space, so the straight line between any 
two endpoints in 0f ree also lies in 0f ree . The straight line can be written 

9(s) = dstart + s(dend — $start), S € [0, 1] (9-3) 


with derivatives 


d9 

ds 


9e nd ^start 



(9.4) 

(9.5) 


Straight lines in joint space generally do not yield straight-line motion of the 
end-effector in task space. If task space straight-line motions are desired, the 
start and end configurations can be specified by X sta rt and A^nd in task space. 
If Agtart and Af en d are represented by a minimum set of coordinates, then a 
straight line is defined as X(s) = Af s tart + s(Af e nd~ Af sta rt),s g [0,1]. Compared 
to joint coordinates, however, the following are issues that must be addressed: 

• If the path passes near a kinematic singularity, joint velocities may become 
unreasonably large for almost all time scalings of the path. 


• Since the robot’s reachable task space may not be convex in X coordinates, 
some points on a straight line between two reachable endpoints may not 
be reachable (Figure 9.1). 
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Figure 9.1: (Left) A 2R robot with joint limits 0° < 9\ < 180°, 0° < Qi < 150°. 
(Top right) A straight-line path in joint space and the corresponding motion of 
the end-effector in task space. The reachable endpoint configurations, subject 
to joint limits, are indicated in gray. (Bottom right) A straight-line path in task 
space would violate the joint limits. 


In addition to the issues above, if -A start and X en d are represented as elements 
of SE( 3) instead of as a minimum set of coordinates, then there is the question 
of how to define a “straight” line in SE( 3). A configuration of the form X sta rt + 
s(Agnd — A'start) does not generally lie in SE( 3). 

One option is to use the screw motion (simultaneous rotation about and 
translation along a fixed screw axis) that moves the robot’s end-effector from 
Astart = A(0) to A end = A(l). To derive this A(s), we can write the start and 
end configurations explicitly in the {s} frame as A S;Star t and A Si6n d and use our 
subscript cancellation rule to express the end configuration in the start frame: 

A s t a rt,end — A s t a rt,s A s en d — X s A Sjen d ■ 

Then \og(X~'X S: end) is the matrix representation of the twist, expressed in 
the {start} frame, that takes A sta rt to A en d in unit time. The path can therefore 
be written as 

X(s) — -^start exp(log(X st { rt Xend)s) (9.6) 

where A sta rt is post-multiplied by the matrix exponential since the twist is 
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screw path 


X 



Figure 9.2: A path following a constant screw motion vs. a decoupled path where 
the frame origin follows a straight line and the angular velocity is constant. 

represented in the {start} frame, not the fixed world frame {s}. 

This screw motion provides a “straight-line” motion in the sense that the 
screw axis is constant. The origin of the end-effector does not generally follow a 
straight line in Cartesian space, however, since it follows a screw motion. It may 
be preferable to decouple the rotational motion from the translational motion. 
Writing X = (R,p), we can define the path 


— Pstart T ^(Pend Pst art.) 

R(s) = R start exp (log ( R s ^ ;ir t -Rend ) ^ ) 


(9.7) 

(9.8) 


where the frame origin follows a straight line while the axis of rotation is constant 
in the body frame. Figure 9.2 illustrates a screw path and a decoupled path for 
the same X sta rt and W end . 

9.2.2 Time Scaling a Straight-Line Path 

A time scaling s(t) of a path should ensure that the motion is appropriately 
smooth and that any constraints on robot velocity and acceleration are satisfied. 
For a straight-line path in joint space of the form (9.3), the time-scaled joint 
velocities and accelerations are 6 = s(9 en d — 0 sta rt) and 9 = s(0 end — 0 s tart), 
respectively. For a straight-line path in task space, parametrized by minimum 
coordinates X € R m , simply replace 6 1 6 , and 6 by X , X , and X. 

9.2.2.1 Polynomial Time Scaling 

Third-order Polynomials A convenient form for the time scaling s(t) is a 
cubic polynomial of time, 


s(t) = a 0 + ait + a 2 t 2 + a 3 t 3 . 


(9.9) 


A point-to-point motion in time T imposes the initial constraints s(0) = s(0) = 0 
and the terminal constraints s(T) = 1 and s(T) = 0. Evaluating Equation (9.9) 
and its derivative 


s(t) = <Zi + 2a 2 t + 3a 3 t 2 


(9.10) 
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Figure 9.3: Plots of s(t), s(t), and s(t) for a third-order polynomial time scaling. 


at t = 0 and t = T and solving the four constraints for «o,..., < 23 , we find 

3 2 

ao = 0, cl \ = 0, a 2 = ^, 03 = —^. 

Plots of s(t), s(t), and s(t) are shown in Figure 9.3. 

Plugging s = a 2 f 2 + cot 3 into Equation (9.3) yields 

/ 3^2 2 t 3 \ 

9(t) = 0 s tart + ~~ ) (^ end _ ^start) (9.11) 

m=^-^pj(e end -e stait ) (9.i2) 

0(t) = (Jf - (0 e „d - 0start). (9-13) 

The maximum joint velocities are achieved at the halfway point of the motion 
t = T/2: 

3 

^max — 2 Y’ (^end fl.start.) ■ 

The maximum joint accelerations and decelerations are achieved at t = 0 and 
t = T: 



6 


6 

^max 

7j^2 (^end ^start) 

5 ^min — 

7^2 end ^start) 


If there are known limits on the maximum joint velocities \0\ < 0n m it and max¬ 
imum joint accelerations \6\ < these bounds can be checked to see if the 

requested motion time T is feasible. Alternatively, T could be solved for to find 
the minimum possible motion time that satisfies the most restrictive velocity or 
acceleration constraint. 

Fifth-order Polynomials Because the third-order time scaling does not con¬ 
strain the endpoint path accelerations s(0) and s(T) to be zero, the robot is 
asked to achieve a discontinuous jump in acceleration at both t = 0 and t = T. 
This implies infinite jerk , the derivative of acceleration, which may cause vibra¬ 
tion of the robot. 
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Figure 9.4: Plots of s(t), s(t), and s(t) for a fifth-order polynomial time scaling. 



Figure 9.5: Plots of s(t) and s(t) for a trapezoidal motion profile. 


One solution is to constrain the endpoint accelerations to s(0) = s(T) = 0. 
The addition of these two constraints requires the addition of two more design 
freedoms in the polynomial, yielding a quintic polynomial of time, s(t) = clq + 
... + a$t 5 . We can use the six terminal position, velocity, and acceleration 
constraints to solve uniquely for ao ... 05 (Exercise 5), which yields a smoother 
motion with a higher maximum velocity than a cubic time scaling. A plot of 
the time scaling is shown in Figure 9.4. 

9.2.2.2 Trapezoidal Motion Profiles 

Trapezoidal time scalings are quite common in motor control, particularly for 
the motion of a single joint, and they get their name from their velocity profiles. 
The point-to-point motion consists of a constant acceleration phase s = a of 
time t a , followed by a constant velocity phase s = v of time t v = T — 2 t a , 
followed by a constant deceleration phase s = —a of time t a . The resulting s 
profile is a trapezoid and the s profile is the concatenation of a parabola, linear 
segment, and parabola as a function of time (Figure 9.5). 

The trapezoidal time scaling is not as smooth as the cubic time scaling, but 
it has the advantage that if there are known limits on joint velocities 0 ii m it 6 R n 
and joint accelerations 0 ii m ;t £ M™, the trapezoidal motion using the largest v 
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and a satisfying 


I(0end - flstartM < (9-14) 

| ($end — ^start )a\ < 6Ui t (9-15) 


is the fastest straight-line motion possible. (See Exercise 8.) 

If v 2 /a > 1, the robot never reaches the velocity v during the motion (Ex¬ 
ercise 10). The three-phase accelerate-coast-decelerate motion becomes a two- 
phase accelerate-decelerate “bang-bang” motion, and the trapezoidal profile s(t) 
in Figure 9.5 becomes a triangle. 

Assuming that v 2 /a < 1, the trapezoidal motion is fully specified by v, a, 
t a , and T, but only two of these can be specified independently, since they 
must satisfy s(T) = 1 and v = at a . It is unlikely that we would specify t a 
independently, so we can eliminate it from the equations of motion by the sub¬ 
stitution t a = v/a. The motion profile during the three stages (acceleration, 
coast, deceleration) can then be written in terms of v, a, and T as 


v 

0 <t< - 
a 


v v 

— <t <T - 

a a 


T - <t<T 

a 


s(t) = a 

(9.16) 

s(t) = at 

(9.17) 

s(t) = -at 2 
v ’ 2 

(9.18) 

s(t) = 0 

(9.19) 

s(t) = V 

(9.20) 

»(t) = vt - V - 

(9.21) 

s(t) = —a 

(9.22) 

s(t) = a(T — t) 

(9.23) 

.. 2avT — 2v 2 — a 2 (t — T) 2 
’ {t) = 2a 

(9.24) 


Since only two of v, a , and T can be chosen independently, we have three 
options: 

• Choose v and a such that v 2 /a < 1, assuring a three-stage trapezoidal 
profile, and solve s(T) = 1 (Equation (9.24)) for T: 


va 

If v and a correspond to the highest possible joint velocities and acceler¬ 
ations, this is the minimum possible time for the motion. 
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Figure 9.6: Plot of s(t) for an S-curve motion profile consisting of seven stages: 
(1) constant positive jerk, (2) constant acceleration, (3) constant negative jerk, 
(4) constant velocity, (5) constant negative jerk, ( 6 ) constant deceleration, and 
(7) constant positive jerk. 

• Choose v and T such that 2 > vT > 1, assuring a three-stage trapezoidal 
profile and that the top speed v is sufficient to reach s = 1 in time T, and 
solve s(T ) = 1 for a: 


• Choose a and T such that aT 2 > 4, assuring that the motion is completed 
in time, and solve s(T) = 1 for v: 

d = - (aT — \fa\/aT 2 — 4^j . 

9.2.2.3 S-Curve Time Scalings 

Just as cubic polynomial time scalings lead to infinite jerk at the beginning and 
end of the motion, trapezoidal motions cause discontinuous jumps in accelera¬ 
tion at t G {0,f a ,T — i a ,T}. A solution is a smoother S-curve time scaling, a 
popular motion profile in motor control because it avoids vibrations or oscilla¬ 
tions induced by step changes in acceleration. An S-curve time scaling consists 
of seven stages: (1) constant jerk s ( 3 > = J until a desired acceleration s = a is 
achieved; ( 2 ) constant acceleration until the desired s = v is being approached; 
(3) constant negative jerk — J until s equals zero exactly at the time s reaches 
v: (4) coasting at constant v. (5) constant negative jerk — J; ( 6 ) constant decel¬ 
eration —a; and (7) constant positive jerk J until s' and s reach zero exactly at 
the time s reaches 1 . 

The s[t) profile for an S-curve is shown in Figure 9.6. 

Given some subset of v, a, J, and the total motion time T, algebraic ma¬ 
nipulation reveals the switching time between stages and conditions that ensure 
that all seven stages are actually achieved, similar to the case of the trapezoidal 
motion profile. 
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Figure 9.7: Two paths in an (x,y) space corresponding to piecewise-cubic tra¬ 
jectories interpolating four via points, including one start point and one end 
point. The velocities at the start and end are zero, and the velocities at vias 2 
and 3 are indicated by dashed tangent vectors. The shape of the path depends 
on the velocities specified at the via points. 

9.3 Polynomial Via Point Trajectories 

If the goal is to have the robot joints pass through a series of via points at 
specified times, without a strict specification on the shape of path between con¬ 
secutive points, a simple solution is to use polynomial interpolation to directly 
find joint histories 9(t) without first specifying a path 9(s) and then a time 
scaling s(i) (Figure 9.7). 

Let the trajectory be specified by k via points, with the start point occurring 
at Xj = 0 and the final point at T*, = T. Since each joint history is interpolated 
individually, we focus on a single joint variable and call it /3 to avoid proliferation 
of subscripts. At each via point i £ {1...&}, the user specifies the desired 
position /3(Tj) = /3j and velocity $ (Tj) = /%. The trajectory has k— 1 segments, 
and the duration of segment j £ {1,..., k — 1} is A Tj = X) + i — T r The joint 
trajectory during segment j is expressed as the third-order polynomial 

P(Tj H- At) = Gjq T djiAt -f- dj2^t^ T cz^At'^ (9.25) 

in terms of the time At elapsed in segment j , where 0 < At < AT). Segment j 
is subject to the four constraints 


m) = /?, 

PiTj + ATj) = p j+1 


m)=h 

P{Tj + AT,) = /%+!• 
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Figure 9.8: The coordinate time histories for the cubic via point interpolation 
of Figure 9.7(a). 


Solving these constraints for ajo ,..., a ,3 yields 

a j0 = Pj 
a ji = Pi 

3pj+i - 3 pj - 2/3, A Tj - Pj + iATj 
a > 2 ~ -A Jf - 

_ 2/3,- + (Pj + p j+ i)ATj - 2 p j+1 
aj 3 - A Tf • 

Figure 9.8 shows the time histories for the interpolation of Figure 9.7(a). In 
this 2D (x,y) coordinate space, the via points 1...4 occur at times Tj = 0, 
T 2 = 1, T 3 = 2, and T 4 = 3. The via points are at (0, 0), (0,1), (1,1), and (1,0) 
with velocities ( 0 , 0 ), ( 1 , 0 ), ( 0 ,- 1 ), and ( 0 , 0 ). 

Two issues are worth mentioning: 

• The quality of the interpolated trajectories is improved by “reasonable” 
combinations of via point times and via point velocities. For example, 
if the user wants to specify a via point location and time, but not the 
velocity, a heuristic could be used to choose a via velocity based on the 
times and coordinate vectors to the via points before and after the via in 
question. As an example, the trajectory of Figure 9.7(b) is smoother than 
the trajectory of Figure 9.7(a). 

• Cubic via point interpolation ensures that velocities are continuous at via 
points, but not accelerations. The approach is easily generalized to use 
fifth-order polynomials and specifications of the accelerations at the via 
points, at the cost of increased complexity of the solution. 

If only two points are used (the start and end point), and the velocities 
at each are zero, the resulting trajectory is identical to the straight-line cubic 
polynomial time-scaled trajectory discussed in Section 9.2.2.1. 


(9.26) 

(9.27) 

(9.28) 

(9.29) 
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Figure 9.9: A path planner has returned a semicircular path of radius R around 
an obstacle in (x, y) space for a robot with two prismatic joints. The path can 
be represented as a function of a path parameter s as x(s) = x c + R cos sir and 
y(s ) = y c — RsinsTT for s € [0,1]. For a 2R robot, inverse kinematics would be 
used to express the path as a function of s in joint coordinates. 


There are many other methods for interpolating a set of via points. For 
example, B-spline interpolation is popular. In B-spline interpolation, the path 
may not pass exactly through the via points, but the path is guaranteed to be 
confined to the convex hull of the via points, unlike the paths in Figure 9.7. 
This can be important to ensure that joint limits or workspace obstacles are 
respected. 

9.4 Time-Optimal Time Scaling 

In the case that the path 9(s) is fully specified by the task or an obstacle¬ 
avoiding path planner (e.g., Figure 9.9), the trajectory planning problem reduces 
to finding a time scaling s(t). One could choose the time scaling to minimize 
energy consumed while meeting a time constraint, or to prevent spilling a glass 
of water the robot is carrying. One of the most useful time scalings, however, 
is the one that minimizes the time of motion along the path, subject to the 
robot’s actuator limits. Such time-optimal trajectories maximize the robot’s 
productivity. 

While the trapezoidal time scalings of Section 9.2.2.2 can yield time-optimal 
trajectories, this is only under the assumption of straight-line motions and con¬ 
stant maximum acceleration a and maximum coasting velocity v. For most 
robots, because of state-dependent joint actuator limits and the state-dependent 
dynamics 

M(8)8 + c(B,8)+g(8) = r, (9.30) 

the maximum available velocities and accelerations change along the path. 
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In this section we consider the problem of finding the fastest possible time 
scaling s(t) that respects the robot’s actuator limits. We write the limits on the 
1 th actuator as 

r“ in (M) < n < 7f ax (M). (9.31) 

The available actuator torque is typically a function of the current joint speed 
(see Chapter 8.9.1). For example, for a given maximum voltage of a DC motor, 
the maximum torque available from the motor drops linearly with the motor’s 
speed. 

Before proceeding, we note that the quadratic velocity terms c(9, 9) in Equa¬ 
tion (9.30) can be written equivalently as 

c(M) = e T r(e)e 1 

where T(0) is the three-dimensional tensor of Christoffel symbols constructed 
of partial derivatives of components of the mass matrix M(9) with respect to 
9. This form more clearly shows the quadratic dependence on velocities. Now 
beginning with Equation (9.30), replacing 9 by ( d6/ds)s and 9 by (d9/ds)s + 
( d 2 9/ds 2 )s 2 , and rearranging, we get 

Hi*)»++(s » 2 + e<f»=<“ 2 > 

v -V- ' w _s(s)eK n 

m ( s ) 6K " c(s)e*“ 

expressed more compactly as the vector equation 

m(s)s + c(s)s 2 + g(s) = r, (9.33) 

where m(s) is the effective inertia of the robot confined to the path 9(s), c(s)s 2 
are the quadratic velocity terms, and g(s) is the gravitational torque. 

Similarly, the actuation constraints (9.31) can be expressed as a function of 

C n (M)<r,<Tr x (M). (9.34) 

Plugging in the components of Equation (9.33), we get 

r™ ln (s, s) < rrii(s)s + Ci(s)s 2 + gi{s) < r™ ax (s, s). (9.35) 

Let Lj(s, s) and Ui(s, s) be the minimum and maximum accelerations s sat¬ 
isfying the 1th component of Equation (9.35). Depending on the sign of mj(s), 
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we have three possibilities: 


if Wi(s) > 0 : Lj(s) = 
Ui{s ) = 

if rm(s) < 0 : Li{s) = 
Ui(s) = 


T™ in (s,s) - c{s)s 2 -g(s) 
rrii(s) 

rr x (s,s) - c(s)s 2 -g(s) 
rrii(s) 

r™ ax (s, s) — c(s)s 2 — g(s) 
rrii(s) 

T™ in (s,s) - c(s)s 2 -g(s) 
rrii(s) 


(9.36) 


if rrii(s) = 0 : zero-inertia point , discussed in Section 9.4.3 


Defining 


L(s, s) = maxLj(s, s) and U(s, s) = min Ui(s, s), 

i i 

the actuator limits (9.35) can be written as the state-dependent time-scaling 
constraints 

L(a, s) < s < U(s, s). (9.37) 

The time-optimal time-scaling problem can now be stated: 

Given a path 9(s),s £ [0,1], an initial state (so,sq) = (0,0), and a final state 
(.Sf , Sf) = (1,0), find a monotonically increasing twice-differentiable time scaling 
s : [0, T] —> [0,1] that 

(i) satisfies s(0) = s(0) = s(T) = 0 and s(T) = 1, and 

(ii) minimizes the total travel time T along the path while respecting the actu¬ 
ator constraints (9.37). 

The problem formulation is easily generalized to the case of nonzero initial 
and final velocity along the path, s(0) > 0 and s(T) > 0. 

9.4.1 The (s, s) Phase Plane 

The problem is easily visualized in the (s, s) phase plane of the path-constrained 
robot, with s running from 0 to 1 on a horizontal axis and s on a vertical 
axis. Since s(t) is monotonically increasing, s(t) > 0 for all times t and for all 
s € [0,1]. A time scaling of the path is any curve in the phase plane that moves 
monotonically to the right from (0, 0) to (1,0) (Figure 9.10). Not all such curves 
satisfy the acceleration constraints (9.37), however. 

To see the effect of the acceleration constraints, at each (s, s) in the phase 
plane, we can plot the limits L(s,s ) < s < U(s,s) as a cone, as illustrated in 
Figure 9.11(a). If L(s,s ) > U(s,s), the cone disappears—there are no actuator 
commands that can keep the robot on the path at this state. These inadmissible 
states are indicated in gray in Figure 9.11(a). For any s, typically there is a 
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Figure 9.10: A time scaling in the (s, s) phase plane is a curve with s > 0 at all 
times connecting the initial path position and velocity (0, 0) to the final position 
and velocity (1,0). 




Figure 9.11: (a) Acceleration-limited motion cones at four different states. The 
upper ray of the cone is the sum of U(s,s) plotted in the vertical direction 
(change in velocity) and s plotted in the horizontal direction (change in posi¬ 
tion). The lower ray of the cone is constructed from L(s,s) and s. Points in 
gray, bounded by the velocity limit curve, have L(s,s ) > U(s,s )—the state is 
inadmissible. On the velocity limit curve, the cone is reduced to a single tangent 
vector, (b) The proposed time scaling is infeasible because the tangent to the 
curve is outside the motion cone at the state indicated. 


single limit velocity sii m (s) above which all velocities are inadmissible. The 
function Sii m (s) is called the velocity limit curve. On the velocity limit curve, 
L(s, s ) = C/(s, s), and the cone reduces to a single vector. 

For a time scaling to satisfy the acceleration constraints, the tangent of the 
time-scaling curve must lie inside the feasible cone at all points on the curve. 
This shows that the time scaling in Figure 9.11(b) is infeasible; it demands more 
deceleration than the actuators can provide at the state indicated. 

For a minimum-time motion, the “speed” s must be as high as possible 
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Figure 9.12: (a) A time-optimal bang-bang time scaling integrates U(s,s) from 
(0,0) and switches to L(s, s) at a switching point s*. Also shown is a non- 
optimal time scaling with a tangent inside a motion cone, (b) Sometimes the 
velocity limit curve prevents a single-switch solution. 


at every s while still satisfying the acceleration constraints and the endpoint 
constraints. To see this, consider that the total time of motion T is given by 


T = 



(9.38) 


Making the substitution ds/ds = 1, and changing the limits of integration from 
0 to T (time) to 0 to 1 (s), we get 


/*T nT j j>T 7 ^ /»1 

T= 1 dt= — dt = I —ds= s~ 1 (s) ds. (9.39) 

Jo Jo ds Jo ds J o 

Thus for time to be minimized, s -1 (s) should be as small as possible, and 
therefore s(s) must be as large as possible, at all s, while still satisfying the 
acceleration constraints (9.37) and the boundary constraints. 

This implies that the time scaling must always operate at the limit U(s , s) or 
L(s, s), and our only choice is when to switch between these limits. A common 
solution is a bang-bang trajectory: maximum acceleration U(s,s) followed by 
a switch to maximum deceleration L(s, s). (This is similar to the trapezoidal 
motion profile that never reaches the coasting velocity v in Section 9.2.2.2.) 
In this case, the time scaling is calculated by numerically integrating U(s,s) 
forward in s from (0, 0), integrating L(s, s) backward in s from (1,0), and finding 
the intersection of these curves (Figure 9.12(a)). The switch between maximum 
acceleration and maximum deceleration occurs at the intersection. 

In some cases, however, the velocity limit curve prevents a single-switch 
solution (Figure 9.12(b)). These cases require an algorithm to find multiple 
switching points. 
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9.4.2 The Time-Scaling Algorithm 

Finding the optimal time scaling is reduced to finding the switches between 
maximum acceleration U(s,s ) and maximum deceleration L(s, s), maximizing 
the “height” of the curve in the ( s , s) phase plane. 

Time-scaling algorithm. 

1 . Initialize an empty list of switches S = {} and a switch counter * = 0. Set 

( Si, Si ) = ( 0 , 0 ). 

2. Integrate the equation s = L(s, s ) backward in time from (1,0) until L(s, s) > 
U (s, s) (the velocity limit curve is penetrated) or s = 0. Call this phase plane 
curve F. 

3. Integrate the equation s = U (s, s) forward in time from (sj, Sj) until it crosses 
F or until U(s, s) < L(s, s) (the velocity limit curve is penetrated). Call this 
curve Aj. If Aj crosses F , then increment i, set (Sj,Sj) to the (s, s) value at 
which the crossing occurs, and append Sj to the list of switches S. This is a 
switch from maximum acceleration to maximum deceleration. The problem 
is solved and S is the set of switches expressed in the path parameter. If 
instead the velocity limit curve is penetrated, let (sn m , Sn m ) be the point of 
penetration and proceed to the next step. 

4. Perform a binary search on the velocity in the range [0, sii m ] to find the 
velocity s' such that the curve integrating s = L{s , s) forward from (su m , s') 
touches the velocity limit curve without penetrating it. The binary search is 
initiated with Shigh = su m and si ow = 0. 

(a) Set the test velocity halfway between si ow and Shigh, Stest = (shigh + 
Siow)/2. The test point is (si im , s tes t)- 

(b) If the curve from the test point penetrates the velocity limit curve, set 
Shigh equal to s tes t- If instead the curve from the test point hits s = 0, 
set Slow equal to St es t- Return to step 4a. 

Continue the binary search until a specified tolerance. Let (stan,Stan) equal 
the point where the resulting curve just touches the velocity limit curve 
tangentially (or comes closest to the curve without hitting). The motion 
cone at this point is reduced to a single tangent vector (L(s, s) = U(s,s)), 
tangent to the velocity limit curve. 

5. Integrate s = L(s, s) backwards from (stan> St an ) until it intersects Aj. Incre¬ 
ment i. set (sj, Sj) to the (s, s) value at the intersection, and label as A, the 
curve segment from (sj,Sj) to (stanjStan)- Append Sj to the list of switches 
S. This is a switch from maximum acceleration to maximum deceleration. 

6 . Increment i and set (sj,Sj) to (stanjStan)- Append Sj to the list of switches 
S. This is a switch from maximum deceleration to maximum acceleration. 
Go to step 3. 

The algorithm is illustrated in Figure 9.13. 
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Figure 9.13: The time-scaling algorithm. (Step 2) Integrating s = L(s, s) back¬ 
ward from (1,0) until the velocity limit curve is reached. (Step 3) Integrating 
s = U(s,s) forward from (0,0) to the intersection (sii m ,sn m ) with the velocity 
limit curve. (Step 4) Binary search to find (sum, s 7 ) from which s = L(s,s), 
integrated forward from (siimji 7 ), touches the velocity limit curve tangentially. 
(Step 5) Integrating backward along L(s,s ) from (stanjStan) to find the first 
switch from acceleration to deceleration. (Step 6 ) The second switch, from de¬ 
celeration to acceleration, is at (s 2 ,S 2 ) = (stan>s ta n)- (Step 3) Integrating for¬ 
ward along U(s, s) from (s 2 , S 2 ) results in intersection with F at (S 3 , S 3 ), where 
a switch occurs from acceleration to deceleration. The optimal time scaling 
consists of switches at S = {si, S 2 , S 3 }. 
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9.4.3 Assumptions and Caveats 

The description above covers the major points of the optimal time-scaling algo¬ 
rithm. A few assumptions were glossed over, however; they are made explicit 
below. 

• Static posture maintenance. The algorithm, as described, assumes that the 
robot can maintain its configuration against gravity at any state (s, s = 0). 
This ensures the existence of valid time scalings, namely, time scalings 
that move the robot along the path arbitrarily slowly. For some robots 
and paths, this assumption may be violated due to weak actuators. For 
example, some paths may require momentum to carry motion through 
configurations the robot cannot maintain statically. The algorithm can be 
modified to handle such cases. 

• Inadmissible states. The algorithm assumes that at every s there is a 
unique velocity limit sii m (s) > 0 such that all velocities s < sii m (s) are 
admissible and all velocities s > sii m (s) are inadmissible. For some models 
of actuator dynamics or friction, this assumption may be violated—there 
may be isolated “islands” of inadmissible states. The algorithm can be 
modified to handle this case. 

• Zero-inertia points. The algorithm assumes no zero-inertia points (Equa¬ 
tion (9.36)). If m,i(s) = 0 in (9.36), then the torque provided by actuator 
i has no dependence on the acceleration s, and the zth actuator constraint 
in (9.35) directly defines a velocity constraint on s. At a point s with 
one or more zero components in m(s), the velocity limit curve is defined 
by the minimum of (a) the velocity constraints defined by the zero-inertia 
components and (b) the s values satisfying Lj(s, s) = Ui(s, s) for the other 
components. For the algorithm as described, singular arcs of zero-inertia 
points on the velocity limit curve may lead to rapid switching between 
s = U(s,s) and s = L(s,s). In such cases, choosing an acceleration tan¬ 
gent to the velocity limit curve, and between U ( s, s) and L(s, s), preserves 
time optimality without chattering controls. 

It is worth noting that the time-scaling algorithm generates trajectories with 
discontinuous acceleration, which could lead to vibrations. Beyond this, inaccu¬ 
racies in models of robot inertial properties and friction make direct application 
of the time-scaling algorithm impractical. Finally, since a minimum-time time 
scaling always saturates at least one actuator, if the robot gets off the planned 
trajectory, there may be no torque left for corrective action by a feedback con¬ 
troller. 

Despite these drawbacks, the time-scaling algorithm provides a deep under¬ 
standing of the true maximum capabilities of a robot following a path. 
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9.5 Summary 

• A trajectory 0(t), 9 : [0, T ] —> 0, can be written as 8(s(t)), the composition 
of a path 8(s), 9 : [0,1] —> 0, and a time scaling s(t), s : [0, T] —> [0,1]. 

• A straight-line path in joint space can be written 9(s) = 9 S t ar t + s(0 enc j — 
^start), s£ [0,1]. A similar form holds for straight-line paths in a minimum 
set of task space coordinates. A “straight-line” path in SE( 3), where 
X = ( R,p ), can be decoupled to a Cartesian path and a rotation path: 

P(^) — Pst art. T ^(pend Pst art.) (9.40) 

R(s) = R 

start exp(log(i?J tart l?end)s). (9.41) 

• A cubic polynomial s(t) = ao + ait+d 2 t 2 +d 3 t 3 can be used to time scale a 
point-to-point motion with zero initial and final velocity. The acceleration 
undergoes a step change (infinite jerk) at t = 0 and t = T, however. An 
impulse in jerk can cause vibration of the robot. 

• A quintic polynomial s(t) = + ayt + a, 2 t 2 + arf 3 + ayt 3 + a§t b can be used 

to time-scale a point-to-point motion with zero initial and final velocities 
and accelerations. Jerk is finite at t = 0 and t = T. 

• The trapezoidal motion profile is a popular time scaling in point-to-point 
control, particularly control of a single motor. The motion consists of 
three phases: constant acceleration, constant velocity, and constant decel¬ 
eration, resulting in a trapezoid in s{t). Trapezoidal motion involves step 
changes in acceleration. 

• The S-curve motion profile is also popular in point-to-point control of a 
motor. It consists of seven phases: (1) constant positive jerk, (2) constant 
acceleration, (3) constant negative jerk, (4) constant velocity, (5) constant 
negative jerk, (6) constant deceleration, and (7) constant positive jerk. 

• Given a set of via points including a start state, a goal state, and other 
via states through which the robot’s motion must pass, as well as the 
times Ti these states should be reached, a series of cubic polynomial time 
scalings can be used to generate a trajectory 9{t) interpolating the via 
points. To prevent step changes in the acceleration at the via points, a 
series of quintic polynomials can be used instead. 

• Given a robot path 9{s ), the dynamics of the robot, and limits on the 
actuator torques, the actuator constraints can be expressed in terms of 
(s, s) as the vector inequalities 

L(s, s)s < s <U(s, s). 

The time-optimal time scaling is the s(t) such that the “height” of the 
curve in the (s, s) phase plane is maximized while satisfying s(0) = s(0) = 
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s(T) = 0, s(T ) = 1, and the actuator constraints. The optimal solution al¬ 
ways operates at maximum acceleration U(s , s) or maximum deceleration 
L(s,s). 


9.6 Software 


s = CubicTimeScaling(Tf,t) 

Computes s(t) for a cubic time-scaling, given t and the total time of motion 

Tf- 

s = QuinticTimeScaling(Tf,t) 

Computes s(t) for a quntic time-scaling, given t and the total time of motion 

Tf- 

traj = JointTrajectory(thetastart,thetaend,Tf,N,method) 

Computes a straight-line trajectory in joint space as an TV x n matrix, where 
each of the N rows is an n-vector of joint variables at an instant in time. The 
first row is 0 sta rt and the iVth row is 0 e nd- The elapsed time between each row 
is Tf/(N — 1). The parameter method is either 3 for a cubic time scaling or 5 
for a quintic time scaling. 

traj = ScrewTrajectory(Xstart,Xend,Tf,N,method) 

Computes a trajectory as a list of N SE( 3) matrices, where each matrix repre¬ 
sents the configuration of the end-effector at an instant in time. The first matrix 
is X s tart j the Nth matrix is X en( j, and the motion is along a constant screw axis. 
The elapsed time between each matrix is Tf/(N— 1). The parameter method is 
either 3 for a cubic time scaling or 5 for a quintic time scaling. 

traj = CartesianTrajectory(thetastart,thetaend,Tf,N,method) 

Computes a trajectory as a list of N SE(3) matrices, where each matrix repre¬ 
sents the configuration of the end-effector at an instant in time. The first matrix 
is Xgtart; the Xth matrix is X elK j, and the origin of the end-effector frame fol¬ 
lows a straight line, decoupled from the rotation. The elapsed time between 
each matrix is Tf/(N — 1). The parameter method is either 3 for a cubic time 
scaling or 5 for a quintic time scaling. 

9.7 Notes and References 

In 1985, Bobrow et al. [12] and Shin and McKay [122] published papers nearly 
simultaneously that independently derived the essence of the time-optimal time¬ 
scaling algorithm outlined in Section 9.4. A year earlier, Hollerbach addressed 
the restricted problem of finding dynamically feasible time-scaled trajectories for 
uniform time scalings where the time variable t is replaced by ct for c > 0 [41]. 

The original papers of Bobrow et al. and Shin and McKay were followed by 
a number of papers refining the methods by addressing singularities, algorithm 
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efficiency, and even the presence of constraints and obstacles [99, 126, 117, 
118, 119, 120, 121]. Other research has focused on numerical methods such as 
dynamic programming or nonlinear optimization to minimize cost functions such 
as actuator energy. One early example of work in this area is by Vukobratovic 
and Kircanski [138]. 
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Figure 9.14: An elliptical path. 


9.8 Exercises 


1. Consider an elliptical path in the (x,y) plane. The path starts at (0,0) 
and proceeds clockwise to (2,1), (4, 0), (2, —1), and back to (0,0) (Figure 9.14). 
Write the path as a function of s £ [0,1], 

2 . A cylindrical path in X = ( x,y,z ) is given by x = cos27rs, y = sin27rs, 
z = 2s, s € [0,1], and its time scaling is s(t) — \t + |f 2 , t £ [0, 2]. Write X and 
X. 

3. Consider a path from X(0) = X start £ SE( 3) to X{1) = X en d £ SE( 3) 
consisting of motion along a constant screw axis. The path is time scaled by 
some s{t). Write the twist V and acceleration V at any point on the path given 
s and s. 

4. Consider a straight-line path 6(s) = 9 S tart + s(0 en d — 0 s tart),s £ [0,1] from 
^start = (0,0) to 0 e nd = (tt, 7t/3). The feasible joint velocities are |^i|, | 02 1 < 
2 rad/s and the feasible joint accelerations are |$i|, \9 2 \ < 0.5 rad/s 2 . Find the 
fastest motion time T using a cubic time scaling that satisfies the joint velocity 
and acceleration limits. 

5. Find the fifth-order polynomial time scaling that satisfies s(T) = 1 and 
s(0) = s(0) = s(0) = s(T) = s(T) = 0. 

6 . Asa function of the total time of motion T, find the times at which the accel¬ 
eration s of the fifth-order polynomial point-to-point time scaling is maximum 
and minimum. 

7. If you want to use a polynomial time scaling for point-to-point motion 
with zero initial and final velocity, acceleration, and jerk, what would be the 
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minimum order of the polynomial? 

8 . Prove that the trapezoidal time scaling, using the maximum allowable ac¬ 
celeration a and velocity v, minimizes the time of motion T. 

9. Plot by hand the acceleration profile s(f) for a trapezoidal time scaling. 

10. If v and a are specified for a trapezoidal time scaling, prove that v 2 /a < 1 
is a necessary condition for the robot to reach the maximum velocity v during 
the path. 

11. If v and T are specified for a trapezoidal time scaling, prove that vT > 1 
is a necessary condition for the motion to be able to complete in time T. Prove 
that vT < 2 is a necessary condition for a three-stage trapezoidal motion. 

12. If a and T are specified for a trapezoidal time scaling, prove that aT 2 > 4 
is a necessary condition to ensure that the motion completes in time. 

13. Consider the case where the maximum velocity v is never reached in a 
trapezoidal time scaling. The motion becomes a bang-bang motion: constant 
acceleration a for time Tj 2 followed by constant deceleration —a for time T/ 2. 
Write the position s(t), velocity s{t ), and acceleration s(t) for both phases, 
analogous to Equations (9.16)-(9.24). 

14. Plot by hand the acceleration profile s(t) for an S-curve time scaling. 

15. A seven-stage S-curve is fully specified by the time t j (duration of constant 
positive or negative jerk), the time t a (duration of constant positive or negative 
acceleration), the time t v (duration of constant velocity), the total time T, the 
jerk J, the acceleration a, and the velocity v. Of these seven quantities, how 
many can be specified independently? You may assume that any inequality 
constraints needed for a seven-stage profile are satisfied. 

16. A nominal S-curve has seven stages, but it can have fewer if certain inequal¬ 
ity constraints are not satisfied. Indicate which cases are possible with fewer 
than seven stages. By hand, approximately draw the s(t ) velocity profiles for 
these cases. 

17. If the S-curve achieves all seven stages and uses jerk J, acceleration a, and 
velocity v , what is the constant velocity coasting time t v in terms of v , a, J, 
and the total motion time T? 

18. Write your own via-point cubic polynomial interpolation trajectory gener- 


312 


Trajectory Generation 



Figure 9.15: A, B, and C are candidate integral curves, originating from the 
dots indicated, while a, b, and c are candidate motion cones at s = 0. Two of 
the integral curves and two of the motion cones are incorrect. 

ator program for a 2-DOF robot. A new position and velocity specification is 
required for each joint at 1000 Hz. The user specifies a sequence of via point 
positions, velocities, and times, and the program generates an array consisting 
of the joint angles and velocities at every 1 ms from time t = 0 to time t = T, 
the total duration of the movement. For a test case with at least three via 
points (start and end at rest, and at least one more via), plot (a) the path in 
the joint angle space (similar to Figure 9.7) and (b) the position and velocity of 
each joint as a function of time (similar to Figure 9.8). 

19. Via points with specified positions, velocities, and accelerations can be 
interpolated using fifth-order polynomials of time. For a fifth-order polynomial 
segment between via points j and j + 1, of duration AT,-, with /3j, f3j+i, $j+i, 
j3j, and /3,+i specified, solve for the coefficients of the fifth-order polynomial 
(similar to Equations (9.26)-(9.29)). A symbolic math solver will simplify the 
problem. 

20. By hand or by computer, plot a trapezoidal motion profile in the (s, s) 
plane. 

21. Figure 9.15 shows three candidate motion curves in the (s, s) plane (A, B, 
and C) and three candidate motion cones at s = 0 (a, b, and c). Two of the 
three curves and two of the three motion cones cannot be correct for any robot 
dynamics. Indicate which are incorrect and explain your reasoning. Explain 
why the others are possibilities. 

22. Under the assumptions of Section 9.4.3, explain why the time-scaling al¬ 
gorithm of Section 9.4.2 is correct. In particular, (a) explain why in the binary 
search of Step 4, the curve integrated forward from (sii m , Stest) must either hit 
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(or run tangent to) the velocity limit curve or hit the s = 0 axis (and does not 
hit the curve F, for example); (b) explain why the final time scaling can only 
touch the velocity limit curve tangentially; and (c) explain why the acceleration 
switches from minimum to maximum at points where the time scaling touches 
the velocity limit curve. 

23. Explain how the time-scaling algorithm should be modified, if at all, to 
handle the case where the initial and final velocity, at s = 0 and s = 1 , are 
nonzero. 

24. Explain how the time-scaling algorithm should be modified if the robot’s 
actuators are too weak to hold it statically at some configurations of the path 
(static posture maintenance assumption is violated), but the assumptions on 
inadmissible states and zero-inertia points are satisfied. Valid time scalings 
may no longer exist. Under what condition(s) should the algorithm terminate 
and indicate that no valid time scaling exists? (Under the assumptions of Sec¬ 
tion 9.4.3, the original algorithm always finds a solution, and therefore does not 
check for failure cases.) What do the motion cones look like at states (s, s = 0) 
where the robot cannot hold itself statically? 

25. Create a computer program that plots the motion cones in the (s,s) plane 
for a 2R robot in a horizontal plane. The path is a straight line in joint space 
from ( 61 , 62 ) = (0,0) to (7r/2,7r/2). First derive the dynamics of the arm, then 
rewrite the dynamics in terms of s,s,s instead of 6 , 6 , 6 . The actuators can 
provide torques in the range —— bdi < Ti < Tijimit — bdi, where b > 0 
indicates the velocity dependence of the torque. The cones should be drawn at 
a grid of points in ( s,s ). To keep the figure manageable, normalize each cone 
ray to the same length. 
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Chapter 10 

Motion Planning 


Motion planning is the problem of finding a robot motion from a start state to 
a goal state while avoiding obstacles in the environment and satisfying other 
constraints, such as joint limits or torque limits. Motion planning is one of the 
most active subfields of robotics, and it is the subject of entire books. The 
purpose of this chapter is to provide a practical overview of a few common 
techniques, using robot arms and mobile robots as the primary example systems 
(Figure 10.1). 

The chapter begins with a brief overview of motion planning, followed by 
foundational material including configuration space obstacles, and concludes 
with summaries of several different planning methods. 

10.1 Overview of Motion Planning 

A key concept in motion planning is configuration space, or C-space for short. 
Every point in the C-space C corresponds to a unique configuration q of the 
robot, and every configuration of the robot can be represented as a point in 



Figure 10.1: (Left) A robot arm executing an obstacle-avoiding motion plan. 
The motion plan was generated using Movelt! [132] and visualized using rviz in 
ROS (the Robot Operating System). (Right) A car-like mobile robot parallel 
parking. 
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C-space. For example, the configuration of a robot arm with n joints can be 
represented as a list of n joint positions, q = (6 1 ,..., 0 n ). The free C-space Cf ree 
consists of the configurations where the robot does not penetrate any obstacle 
nor violate a joint limit. 

In this chapter, unless otherwise stated, we assume that q is an n- vector and 
that C C M". With some generalization, however, the concepts of this chapter 
apply to non-Euclidean C-spaces like C = SE( 3) (n = 6 ). 

The control inputs available to drive the robot are written as an m-vector 
u £ U C R m , where m = n for a typical robot arm. If the robot has second- 
order dynamics, like a robot arm, and the control inputs are forces (equivalently, 
accelerations), the state of the robot is its configuration and velocity, x = ( q , v ) 6 
X. For q £ R n , typically we write v = q. If we can treat the control inputs as 
velocities, the state x is simply the configuration q. The notation q(x) indicates 
the configuration q corresponding to the state x, and ftf ree = {x | q{x) £ Cf ree }- 

The equations of motion of the robot are written 


x = f(x,u) 


( 10 . 1 ) 


or, in integral form, 



( 10 . 2 ) 


10.1.1 Types of Motion Planning Problems 

Given the definitions above, a fairly broad specification of the motion planning 
problem is the following: 

Given an initial state a;(0) = cc s tart and a desired final state 2 goa i, find a 
time T and a set of controls u : [0, T] —> IA such that the motion (10.2) satisfies 
x(T ) = Xgoai and q{x{t)) £ Cf ree for all t £ [ 0 ,T]. 

It is assumed that a feedback controller (Chapter 11) is available to ensure 
that the planned motion a fit), t £ [0,T], is followed closely. It is also assumed 
that an accurate geometric model of the robot and environment is available to 
evaluate Cf ree during motion planning. 

There are many variations of the basic problem; some are discussed below. 

Path planning vs. motion planning. The path planning problem is a sub¬ 
problem of the general motion planning problem. Path planning is the 
purely geometric problem of finding a collision-free path q(s),s £ [ 0 , 1 ], 
from a start configuration g(0) = <? s tart to a goal configuration q( 1 ) = g g0 ah 
without concern for dynamics, the duration of motion, or constraints on 
the motion or control inputs. It is assumed that the path returned by the 
path planner can be time scaled to create a feasible trajectory (Chapter 9). 
This problem is sometimes called the piano mover’s problem , emphasizing 
the focus on the geometry of cluttered spaces. 
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Control inputs: m = n vs. m < n. If there are fewer control inputs m than 
degrees of freedom n , the robot is incapable of following many paths, even 
if they are collision-free. For example, a car has n = 3 (position and 
orientation of the chassis in the plane) but m = 2 (forward/backward 
motion and steering)- it cannot slide directly sideways into a parking 
space. 

Online vs. offline. A motion planning problem requiring an immediate result, 
perhaps because obstacles appear, disappear, or move unpredictably, calls 
for a fast, online planner. If the environment is static, a slower offline 
planner may suffice. 

Optimal vs. satisficing. In addition to reaching the goal state, we might want 
the motion plan to minimize (or approximately minimize) a cost J, e.g., 



For example, minimizing with L = 1 yields a time-optimal motion while 
minimizing with L = u T (r)u(r) yields a “minimum-effort” motion. 

Exact vs. approximate. We may be satisfied with a final state x(T) that is 
sufficiently close to x goa i, e.g., ||x(T) - x g0 ai|| < e. 

With or without obstacles. The motion planning problem can be challeng¬ 
ing even in the absence of obstacles, particularly if m < n or optimality is 
desired. 

10.1.2 Properties of Motion Planners 

Planners must conform to the properties of the motion planning problem as 

outlined above. In addition, planners can be distinguished by the following 

properties: 

Multiple-query vs. single-query planning. If the environment is unchang¬ 
ing and the robot will be asked to solve a number of motion planning 
problems in the environment, it may be worth spending the time to build 
a data structure that accurately represents Cf ree - This data structure can 
then be searched to efficiently solve multiple planning queries. Single¬ 
query planners solve each new problem from scratch. 

“Anytime” planning. An anytime planner is one that continues to look for 
better solutions after a first solution is found. The planner can be stopped 
at any time, for example when a specified time limit has passed, and the 
best solution is returned. 

Completeness. A motion planner is said to be complete if it is guaranteed to 
find a solution in finite time if one exists, and to report failure if there is 
no feasible motion plan. A weaker notion is resolution completeness. 
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A planner is resolution complete if it is guaranteed to find a solution if 
one exists at the resolution of a discretized representation of the problem, 
such as the resolution of a grid representation of Cf ree . Finally, a planner 
is probabilistically complete if the probability of finding a solution, if 
one exists, tends to 1 as planning time goes to infinity. 

Computational complexity. The computational complexity of a planner refers 
to characterizations of the amount of time the planner takes to run or the 
amount of memory it requires. These are measured in terms of the de¬ 
scription of the planning problem, such as the dimension of the C-space 
or the number of vertices in the representation of the robot and obsta¬ 
cles. For example, the time for a planner to run may be exponential in 
n, the dimension of the C-space. The computational complexity may be 
expressed in terms of the average case or the worst case. Some planning 
algorithms lend themselves easily to computational complexity analysis, 
while others do not. 

10.1.3 Motion Planning Methods 

There is no single planner applicable to all motion planning problems. Below 

is a broad overview of some of the many motion planners available. Details are 

left to the sections indicated. 

Complete methods (Section 10.3). These methods focus on exact repre¬ 
sentations of the geometry or topology of Cf ree , ensuring completeness. 
For all but simple or low-degree-of-freedom problems, these representa¬ 
tions are mathematically or computationally prohibitive to derive. 

Grid methods (Section 10.4). These methods discretize Cf ree into a grid and 
search the grid for a motion from g sta rt to a grid point in the goal region. 
Modifications of the approach may discretize the state space or control 
space, or use multi-scale grids to refine the representation of Cf ree near 
obstacles. These methods are relatively easy to implement and can return 
optimal solutions, but for a fixed resolution, the memory required to store 
the grid, and the time to search it, grows exponentially with the number 
of dimensions of the space. This limits the approach to low-dimensional 
problems. 

Sampling methods (Section 10.5). A generic sampling method relies on a 
random or deterministic function to choose a sample from the C-space 
or state space; a function to evaluate whether the sample is in Af ree ; a 
function to determine the “closest” previous free-space sample; and a lo¬ 
cal planner to try to connect to, or move toward, the new sample from 
the previous sample. This process builds up a graph or tree representing 
feasible motions of the robot. Sampling methods are easy to implement, 
tend to be probabilistically complete, and can even solve high-degree-of- 
freedom motion planning problems. The solutions tend to be satisficing, 
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not optimal, and it can be difficult to characterize the computational com¬ 
plexity. 

Virtual potential fields (Section 10.6). Virtual potential fields create forces 
on the robot that pull it toward the goal and push it away from obstacles. 
The approach is relatively easy to implement, even for high-degree-of- 
freedom systems, and fast to evaluate, often allowing online implementa¬ 
tion. The drawback is local minima in the potential function: the robot 
may get stuck in configurations where the attractive and repulsive forces 
cancel, but the robot is not at the goal state. 

Nonlinear optimization (Section 10.7). The motion planning problem can 
be converted to a nonlinear optimization problem by representing the path 
or controls by a finite number of design parameters, such as the coefficients 
of a polynomial or a Fourier series. The problem is to solve for the design 
parameters that minimize a cost function while satisfying constraints on 
the controls, obstacles, and goal. While these methods can produce near- 
optimal solutions, they require an initial guess at the solution. Because 
the objective function and feasible solution space are generally not convex, 
the optimization process can get stuck far away from a solution, let alone 
an optimal solution. 

Smoothing (Section 10.8). Often the motions found by a planner are jerky. 
A smoothing algorithm can be run on the result of the motion planner to 
improve the smoothness. 

The major trend in recent years has been toward sampling methods, which 
are easy to implement and can handle high-dimensional problems. 

10.2 Foundations 

Before discussing motion planning algorithms, we establish concepts used in 
many of them: configuration space obstacles, collision detection, graphs, and 
graph search. 

10.2.1 Configuration Space Obstacles 

Determining whether a robot at a configuration q is in collision with a known 
environment generally requires a complex operation involving a CAD model of 
the environment and robot. There are a number of free and commercial software 
packages that can perform this operation, and we will not delve into them here. 
For our purposes, it is enough to know that the workspace obstacles partition 
the configuration space C into two sets, the free space Cf ree and the obstacle 
space Cobs, where C = Cf re eU^obs- Joint limits are treated as obstacles in the 
configuration space. 

With the concepts of Cf ree and C 0 bs, the path planning problem becomes the 
problem of finding a path for a point robot among the obstacles C Q bs- If the 
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Figure 10.2: (Left) The joint angles of a 2R robot arm. (Middle) The arm 
navigating among obstacles. (Right) The same motion in C-space. 



obstacles break Cf ree into disconnected connected components, and g s tart and 
9 goai do not he in the same connected component, then there is no collision-free 
path. 

The explicit mathematical representation of a C-obstacle can be exceedingly 
complex, and for that reason C-obstacles are rarely represented exactly. Despite 
this, the concept of C-obstacles is very important for understanding motion 
planning algorithms. The ideas are best illustrated by examples. 

10.2.1.1 A 2R Planar Arm 

Figure 10.2 shows a 2R planar robot arm, with configuration q = (9 1 , $ 2 ), among 
obstacles A, B, and C in the workspace. The C-space of the robot is represented 
by a portion of the plane with 0 < 9\ < 27r, 0 < 82 < 27r. In fact, however, the 
topology of the C-space is a torus (or doughnut), since the edge of the square 
at 9\ = 27t is connected to the edge 9\ = 0; similarly, 6*2 = 2ir is connected 
to 62 = 0. The square region of R 2 is obtained by slicing the surface of the 
doughnut twice, at 9\ = 0 and 82 = 0 , and laying it flat on the plane. 

The C-space in Figure 10.2 shows the workspace obstacles A, B, and C 
represented as C-obstacles. Any configuration inside a C-obstacle corresponds 
to penetration of the obstacle by the robot arm in the workspace. A free path for 
the robot arm from one configuration to another is shown in both the workspace 
and C-space. The path and obstacles illustrate the topology of the C-space. 
Note that the obstacles break Cf ree into three connected components. 

10.2.1.2 A Circular Planar Mobile Robot 

Figure 10.3 shows a top view of a circular mobile robot whose configuration is 
given by the location of its center, (x,y) £ R 2 . The robot translates in a plane 
with a single obstacle. The corresponding C-obstacle is obtained by “growing” 
the workspace obstacle by the radius of the mobile robot. Any point outside 
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(a) (b) 


Figure 10.3: (a) A circular mobile robot (white) and a workspace obstacle (gray). 
The configuration of the robot is represented by (x,y), the center of the robot, 
(b) In the C-space, the obstacle is “grown” by the radius of the robot and the 
robot is treated as a point. Any (x, y ) configuration outside the dark boundary 
is collision-free. 



Figure 10.4: The grown C-space obstacles corresponding to two workspace ob¬ 
stacles and a circular mobile robot. The overlapping boundaries mean that the 
robot cannot move between the two obstacles. 


this C-obstacle represents a free configuration of the robot. Figure 10.4 shows 
the workspace and C-space for two obstacles, indicating that the mobile robot 
cannot pass between the two obstacles. 

10.2.1.3 A Polygonal Planar Mobile Robot That Translates 

Figure 10.5 shows the C-obstacle for a polygonal mobile robot translating in 
the presence of a polygonal obstacle. The C-obstacle is obtained by sliding the 
robot along the boundary of the of the obstacle and tracing the position of the 
robot’s reference point. 
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Figure 10.5: (a) The configuration of a triangular mobile robot, which can 
translate but not rotate, is represented by the (x, y) location of a reference 
point. Also shown is a workspace obstacle in gray, (b) The corresponding 
C-space obstacle is obtained by sliding the robot around the boundary of the 
obstacle and tracing the position of the reference point. 


10.2.1.4 A Polygonal Planar Mobile Robot That Translates and Rotates 

Figure 10.6 illustrates the C-obstacle for the workspace obstacle and triangular 
mobile robot of Figure 10.5 if the robot is now allowed to rotate. The C-space 
is now three-dimensional, given by (x,y,0) £ R 2 x S 1 . The three-dimensional 
C-obstacle is the union of two-dimensional C-obstacle slices at angles 6 G [0, 2-7t). 
Even for this relatively low-dimensional C-space, an exact representation of the 
C-obstacle is quite complex. For this reason, C-obstacles are rarely described 
exactly. 
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Figure 10.6: (Top) A triangular mobile robot that can rotate and translate, 
represented by the configuration (x,y,0). (Left) The C-space obstacle from 
Figure 10.5(b) when the robot is restricted to 0 = 0. (Right) The full 3-D 
C-space obstacle shown in slices at 10° increments. 

10.2.2 Distance to Obstacles and Collision Detection 

Given a C-obstacle B and a configuration q , let d{q , B) be the distance between 
the robot and the obstacle, where 

d(q,B) >0 no contact with the obstacle 

d(q,B)= 0 contact 

d(q,B) < 0 penetration. 

The distance could be defined as the Euclidean distance between the two closest 
points of the robot and the obstacle. 

A distance-measurement algorithm is one that determines d(q,B). A 
collision-detection routine determines whether d(q, Bj) < 0 for any C-obstacle 
B,. A collision-detection routine returns a binary result, and may or may not 
utilize a distance-measurement algorithm at its core. 

One popular distance-measurement algorithm is called the GJK (Gilbert- 
Johnson-Keerthi) algorithm, which efficiently computes the distance between 
two convex bodies, possibly represented by triangular meshes. Any robot or 
obstacle can be treated as the union of multiple convex bodies. Extensions of 
this algorithm are used in many distance-measurement algorithms and collision- 
detection routines for robotics, graphics, and game physics engines. 

A simpler approach is to approximate the robot and obstacles as unions 
of overlapping spheres. Approximations must always be conservative —the ap¬ 
proximation must cover all points of the object—so that if a collision-detection 
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Figure 10.7: A lamp represented by spheres. The approximation improves as 
the number of spheres used to represent the lamp increases. Figure from [43] 
used with permission. 


routine indicates a free configuration q , we are guaranteed that the actual ge¬ 
ometry is collision-free. As the number of spheres in the representation of the 
robot and obstacles increases, the closer the approximations come to the actual 
geometry. An example is shown in Figure 10.7. 

Given a robot at q represented by ft spheres of radius Ri centered at ri(q), 
i = 1... ft, and an obstacle B represented by t spheres of radius Bj centered at 
bj , j = 1... £, the distance between the robot and the obstacle can be calculated 
as 

d(q,B) = min ||ri(g) - bj || - - Bj. 

Apart from determining whether a particular configuration of the robot is 
in collision, another useful operation is determining whether the robot collides 
during a particular motion segment. While exact solutions have been developed 
for particular object geometries and motion types, the general approach is to 
sample the path at finely spaced points and to “grow” the robot to ensure that 
if two consecutive configurations are collision-free for the grown robot, then 
the swept volume of the actual robot between the two configurations is also 
collision-free. 

10.2.3 Graphs and Trees 

Many motion planners explicitly or implicitly represent the C-space or state 
space as a graph. A graph consists of a collection of nodes A f and a collection 
of edges £, where each edge e connects two nodes. In motion planning, a node 
typically represents a configuration or state, while an edge between nodes n\ and 
ri 2 indicates the ability to move from n\ to n 2 without penetrating an obstacle 
or violating other constraints. 
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(a) 

Figure 10.8: (a) A weighted 
tree. Leaves are shaded gray. 



(c) A 


A graph can be either directed or undirected. In an undirected graph, each 
edge is bidirectional: if the robot can travel from n\ to n 2 , then it can also 
travel from n 2 to n\. In a directed graph, or digraph for short, each edge allows 
travel in only one direction. The same two nodes can have two edges between 
them, in opposite directions. 

Graphs can also be weighted or unweighted. In a weighted graph, each edge 
has its own positive cost associated with traversing it. In an unweighted graph, 
each edge has the same cost (e.g., 1). Thus the most general type of graph we 
consider is a weighted digraph. 

A tree is a digraph in which (1) there are no cycles and (2) each node has at 
most one parent node (i.e., at most one edge leading to the node). A tree has 
one root node with no parents and a number of leaf nodes with no children. 

A digraph, undirected graph, and tree are illustrated in Figure 10.8. 

Given N nodes, any graph can be represented by a matrix A £ S . NxN , where 
element of the matrix represents the cost of the edge from node i to node j 
(a zero indicates no edge between the nodes). A tree can be represented more 
compactly as a list of nodes, each with a link (and perhaps a cost) to at most 
one parent and a list of links (and costs) to its children. 

10.2.4 Graph Search 

Once the free space is represented as a graph, a motion plan can be found by 
searching the graph for a path from the start to the goal. One of the most 
powerful and popular graph search algorithms is A* (pronounced “A star”) 
search. 

10.2.4.1 A* Search 

A* search efficiently finds a minimum-cost path on a graph when the cost of the 
path is simply the sum of the positive edge costs along the path. 

Given a graph described by a set of nodes A f = {l,...,iV}, where node 
1 is the start node, and a set of edges £, A* makes use of the following data 
structures: 
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• a sorted list OPEN of the nodes to be explored from, and a list CLOSED of 
nodes that have already been explored from; 

• a matrix cost [nodel, node2] encoding the set of edges, where the positive 
value corresponds to the cost of moving from nodel to node2 (a negative 
value indicates that no edge exists); 

• an array past_cost [node] of the minimum cost found so far to reach 
node node from the start node; and 

• a search tree defined by an array parent [node], which contains a link for 
each node to the node preceding it in the shortest path found so far from 
the start node to node. 

To initialize the search, the matrix cost is constructed to encode the edges, 
the list OPEN is initialized to the start node 1, the cost to reach the start node 
(past_cost [1] ) is initialized as 0, and past_cost [node] for node £ {2,..., TV} 
is initialized as infinity (or a large number), indicating that we currently have 
no idea of the cost to reach those nodes. 

At each step of the algorithm, the first node in OPEN is removed from OPEN 
and called current. The node current is also added to CLOSED. The first node 
in OPEN is one that minimizes the total estimated cost of the best path to the 
goal that passes through that node, and it is calculated as 

est_total_cost[node] = past_cost[node] + 

heuristic_cost_to_go(node) 

where heuristic_cost_to_go(node) > 0 is an optimistic (underestimating) 
estimate of the actual cost-to-go to the goal from node. For the visibility graph 
example, an appropriate choice for the heuristic is the straight-line distance to 
the goal, ignoring any obstacles. 

Because OPEN is a sorted list according to the estimated total cost, inserting 
a new node at the correct location in OPEN entails a small computational price. 

If the node current is in the goal set, then the search is finished, and the 
path is reconstructed from the parent links. If not, for each neighbor nbr of 
current in the graph, which is not also in CLOSED, the tentative_past_cost 
for nbr is calculated as past_cost [current] + cost [current ,nbr] . If 
tentative_past_cost < past_cost [nbr], then nbr can be reached less ex¬ 
pensively than previously thought, so past_cost [nbr] is set to tentative_past_cost 
and parent [nbr] is set to current. The node nbr is then added (or moved) in 
OPEN according to its estimated total cost. 

The algorithm proceeds by returning to pop off of OPEN the node with the 
lowest estimated total cost. If OPEN is empty, then there is no solution. 

The A* algorithm is guaranteed to return a minimum-cost path, as nodes 
are only checked for inclusion in the goal set when they have the minimum 
total estimated cost of all nodes. If the node current is in the goal set, then 
heuristic_cost_to_go(current) is zero, and since all edge costs are positive, 
we know that any path found in the future must have a cost greater than 
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or equal to past_cost [current] . Therefore the path to current must be a 
shortest path. (There may be other paths of the same cost.) 

If the heuristic cost-to-go is calculated exactly, considering obstacles, then 
A* * will expand the minimum number of nodes necessary to solve the problem. 
Of course, calculating the cost-to-go exactly is equivalent to solving the path 
planning problem, so this is impractical. Instead, the heuristic cost-to-go should 
be calculated quickly and should be as close as possible to the actual cost-to- 
go to ensure that the algorithm runs efficiently. Using an optimistic cost-to-go 
ensures an optimal solution. 

A* is an example of the general class of best-first searches, which always 
explore from the node currently deemed “best” by some measure. Not all types 
of best-first searches are guaranteed to return a minimum-cost path, however. 

The A* search algorithm is described in pseudocode in Algorithm 1. 


Algorithm 1 A* search. 

1: OPEN <- {1} 

2: past_cost [1] <- 0, past_cost [node] <— infinity for node 6 {2,..., N} 

3: while OPEN is not empty do 

4: current t— first node in OPEN, remove from OPEN 

5: add current to CLOSED 

6: if current is in the goal set then 

7: return SUCCESS and the path to current 

8: end if 

9: for each nbr of current not in CLOSED do 

10: tentative_past_cost past_cost[current] + cost[current,nbr] 

11: if tentative_past_cost < past_cost [nbr] then 

12: past_cost[nbr] <— tentative_past_cost 

13: parent[nbr] <— current 

14: put (or move) nbr in sorted list OPEN according to 

est_total_cost[nbr] past_cost[nbr] + 
heuristic_cost_to_go(nbr) 

15: end if 

16: end for 

17: end while 

18: return FAILURE 


10.2.4.2 Other Search Methods 

• Dijkstra’s method. If the heuristic cost-to-go is always estimated as zero, 
then A* always explores from the OPEN node that has been reached with 
minimum past cost. This variant is called Dijkstra’s algorithm , which 
preceded A* historically. Dijkstra’s algorithm is also guaranteed to find a 
minimum-cost path, but on many problems it runs slower than A* due to 
the lack of a heuristic look-ahead function to help guide the search. 
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Breadth-first search. If each edge in £ has the same cost, Dijkstra’s algo¬ 
rithm reduces to breadth-first search. All nodes one edge away from the 
start node are considered first, then all nodes two edges away, etc. The 
first solution found is therefore a minimum-cost path. 

Suboptimal A* search. If the heuristic cost-to-go is overestimated by mul¬ 
tiplying the optimistic heuristic by a constant factor r) > 1, A* search 
will be biased to explore from nodes closer to the goal rather than nodes 
with a low past cost. This may cause a solution to be found more quickly, 
but unlike the case of an optimistic cost-to-go heuristic, the solution will 
not be guaranteed to be optimal. One possibility is to run A* with an 
inflated cost-to-go to find an initial solution, then re-run the search with 
progressively smaller values of r) until the time allotted for the search has 
expired or a solution is found with i) = 1. 

10.3 Complete Path Planners 

Complete path planners rely on an exact representation of Cf ree . These tech¬ 
niques tend to be mathematically and algorithmically sophisticated, and im¬ 
practical for many real systems, so we do not delve into them in detail. 

One approach to complete path planning, which we will see in modified form 
in Section 10.5, is based on representing the complex, high-dimensional space 
Cf r ee by a one-dimensional roadmap R with the following properties: 

(i) Reachability. From every point q £ Cf ree , a free path to a point q' £ R can 
be found trivially (e.g., a straight-line path). 

(ii) Connectivity. For each connected component of Cf ree , there is one con¬ 
nected component of R. 

With such a roadmap, the planner can find a path between any two points in 
the same connected component of Cf ree by simply finding paths from q s tar t to 
a point q' tart £ R, from a point q' goal € R to g goa i, and from q' tart to q' goal on 
the roadmap R. If a path can be found trivially between q sta rt and q'goal, the 
roadmap may not even be used. 

While constructing a roadmap of Cf ree is complex in general, some problems 
admit simple roadmaps. For example, consider a polygon translating among 
polygonal obstacles in the plane. As we have seen in Figure 10.5, the C-obstacles 
in this case are also polygons. A suitable roadmap is the weighted undirected 
visibility graph , with nodes at the vertices of the C-obstacles and edges between 
nodes that can “see” each other (i.e., the line segment between the vertices 
does not intersect an obstacle). The weight associated with each edge is the 
Euclidean distance between the nodes. 

Not only is this a suitable roadmap R 1 but it allows us to use A* search 
to find a shortest path between any two configurations in the same connected 
component of Cf ree , as the shortest path is guaranteed to either be a straight line 
from q s tart to q goa i, or consist of a straight line from q sta rt to a node q' tart £ R 1 a 
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Figure 10.9: (a) The start and goal configurations for a square mobile robot 
(reference point shown) in an environment with a triangular and a rectangular 
obstacle, (b) The grown C-obstacles. (c) The visibility graph roadmap R of 
Cfree- (d) The full graph consists of R plus nodes at g s tart and g g0 ai, along with 
the links connecting these nodes to visible nodes of R. (e) Searching the graph 
results in the shortest path in bold, (f) The robot traversing the path. 

straight line from a node q' goal e R to f7 goa i, and a path along the straight edges 
of R from <?( tart to q' goal (Figure 10.9). Note that the shortest path requires 
the robot to graze the obstacles, so we implicitly treat Cf ree as including its 
boundary. 


10.4 Grid Methods 


A search like A* requires a discretization of the search space. The simplest 
discretization of C-space is a grid. For example, if the configuration space is 
n-dimensional and we desire k grid points along each dimension, the C-space is 
represented by k n grid points. 

The A* algorithm can be used as a path planner for a C-space grid, with 
the following minor modifications: 

• The definition of a “neighbor” of a grid point must be chosen: is the robot 
constrained to move in axis-aligned directions in configuration space, or 
can it move in multiple dimensions simultaneously? For example, for a 
two-dimensional C-space, neighbors could be 4-connected (on the cardi¬ 
nal points of a compass: north, south, east, and west) or 8-connected 
(diagonals allowed), as shown in Figure 10.10(a). If diagonal motions are 
allowed, the cost to diagonal neighbors should be penalized appropriately. 
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0 0i 2 jt 


(a) (b) (c) 

Figure 10.10: (a) A 4-connected grid point and an 8-connected grid point for a 
space n = 2. (b) Grid points spaced at unit intervals. The Euclidean distance 
between the two points indicated is y/5 while the Manhattan distance is 3. (c) A 
grid representation of the C-space and a minimum-length Manhattan-distance 
path for the problem of Figure 10.2. 

For example, the cost to a N/S/E/W neighbor could be 1, while the cost 
to a diagonal neighbor could be \/2. If integers are desired, for efficiency 
of the implementation, the approximate costs 5 and 7 could be used. 

• If only axis-aligned motions are used, the heuristic cost-to-go should be 
based on the Manhattan distance , not the Euclidean distance. The Man¬ 
hattan distance counts the number of “city blocks” that must be trav¬ 
eled, considering that diagonals through a block are not possible (Fig¬ 
ure 10.10(b)). 

• A node nbr is only added to OPEN if the step from current to nbr is 
collision-free. (The step may be considered collision-free if a grown version 
of the robot at nbr does not intersect any obstacles.) 

• Other optimizations are possible due to the known regular structure of 
the grid. 

An A* grid-based path planner is resolution-complete: it will find a solution 
if one exists at the level of discretization of the C-space. The path will be a 
shortest path subject to the allowed motions. 

Figure 10.10(c) illustrates grid-based path planning for the 2R robot example 
of Figure 10.2. The C-space is represented as a grid with k = 32, i.e., a resolution 
of 360°/32 = 11.25° for each joint. This yields a total of 32 2 = 1024 grid points. 

The grid-based planner, as described, is a single-query planner: it solves 
each path planning query from scratch. On the other hand, if the same q goa i 
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Figure 10.11: A wavefront planner on a 2D grid. The goal configuration is 
given a score of 0. Then all collision-free 4-neighbors are given a score of 1. The 
process continues, breadth-first, with each free neighbor (that does not have a 
score already) assigned the score of its parent plus 1. Once every grid cell in 
the connected component of the goal configuration is assigned a score, planning 
from any location in the connected component is trivial: at every step, the 
robot simply moves “downhill” to a neighbor with a lower score. Grid points in 
collision receive a high score. 


will be used in the same environment for multiple path planning queries, it may 
be worth preprocessing the entire grid to enable fast path planning. This is the 
wavefront planner, illustrated in Figure 10.11. 

Although grid-based path planning is easy to implement, it is only appro¬ 
priate for low-dimensional C-spaces. This is because the number of grid points, 
and hence the computational complexity of the path planner, increases expo¬ 
nentially with the number of dimensions n. For instance, a resolution k = 100 in 
a C-space with n = 3 dimensions leads to 1 million grid nodes, while n = 5 leads 
to 10 billion grid nodes and n = 7 leads to 100 trillion nodes. An alternative 
is to reduce the resolution k along each dimension, but this leads to a coarse 
representation of C-space that may miss free paths. 

10.4.1 Multi-Resolution Grid Representation 

One way to reduce the computational complexity of a grid-based planner is to 
use a multi-resolution grid representation of Cf ree . Conceptually, a grid point is 
considered an obstacle if any part of the rectilinear cell centered on the grid point 
touches a C-obstacle. To refine the representation of the obstacle, an obstacle 
cell can be subdivided into smaller cells. Each dimension of the original cell is 
split in half, resulting in 2™ sub-cells for an n-dimensional space. Any of the 
cells that are still in contact with a C-obstacle are then subdivided further, up 
to a specified maximum resolution. 

The advantage of this representation is that only portions of C-space near 
obstacles are refined to high resolution, while portions away from obstacles 
are represented by a coarse resolution. This allows the planner to find paths 
using short steps through cluttered spaces while taking large steps through wide 
open space. The idea is illustrated in Figure 10.12, which uses only 10 cells to 
represent an obstacle at the same resolution as a fixed grid that uses 64 cells. 
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Figure 10.12: At the original C-space cell resolution, a small obstacle (indicated 
by a dark square) causes the whole cell to be labeled an obstacle. Subdividing 
the cell once shows that at least 3/4 of the cell is actually free. Three levels of 
subdivision results in a representation using ten total cells: four at subdivision 
level 3, three at subdivision level 2, and three at subdivision level 1. The cells 
shaded gray are the obstacle cells in the final representation. The subdivision of 
the original cell is also shown as a tree, specifically a quadtree, where the leaves 
of the tree are the final cells in the representation. 


For n = 2, this multiresolution representation is called a quadtree, as each 
obstacle cell subdivides into 2 n = 4 cells. For n = 3, each obstacle cell subdivides 
into 2 n = 8 cells, and the representation is called an octree. 

The multi-resolution representation of Cf ree can be built in advance of the 
search or incrementally as the search is being performed. In the latter case, if 
the step from current to nbr is found to be in collision, the step size can be 
halved until the step is free or the minimum step size is reached. 

10.4.2 Grid Methods with Motion Constraints 

The previous grid-based planners operate under the assumption that the robot 
can go from a cell to any neighbor cell in a regular C-space grid. This may 
not be possible for some robots. For example, a car cannot reach, in one step, 
a “neighbor” cell that is to the side of it. Also, motions for a fast-moving 
robot arm should be planned in the state space, not just C-space, to take the 
arm dynamics into account. In the state space, the robot arm cannot move in 
certain directions (Figure 10.13). 

Grid-based planners must be adapted to account for the motion constraints 
of the particular robot. In particular, the constraints may result in a directed 
grid graph. One approach is to discretize the robot controls while still making 
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Figure 10.13: Sample trajectories emanating from three initial states in the 
phase space of a dynamic system with q £ 1R. If the initial state has q > 0, 
the trajectory cannot move to the left (negative motion in q) instantaneously. 
Similarly, if the initial state has q < 0, the trajectory cannot move to the right 
inst ant aneously. 
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Figure 10.14: Discretizations 
car-like robots. 




of the control sets for unicycle, diff-drive, and 


use of a grid on the C-space or state space, as appropriate. Details for a wheeled 
mobile robot and a dynamic robot arm are described next. 

10.4.2.1 Grid-Based Path Planning for a Wheeled Mobile Robot 

As described in Chapter 13.3, the controls for simplified models of unicycle, 
diff-drive, and car-like robots are (v,ui), the forward-backward linear velocity 
and the angular velocity. The control sets for these mobile robots are shown in 
Figure 10.14. Also shown are proposed discretizations of the controls, as dots. 
Other discretizations could be chosen. 

Using the control discretization, we can use a variant of Dijkstra’s algorithm 
to find approximately shortest paths (Algorithm 2). 

The search expands from g sta rt by integrating forward each of the controls 
for a time At, creating new nodes for the paths that are collision-free. Each 
node keeps track of the control used to reach the node as well as the cost of the 
path to the node. The cost of the path to a new node is the sum of the cost of 
the previous node current plus the cost of the action. 
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Algorithm 2 Grid-based Dijkstra planner for a wheeled mobile robot. 

1: OPEN i {^start} 

2: past_COSt [f/ s tart] 0 
3: counter -S— 1 

4: while OPEN is not empty and counter < MAXCOUNT do 
5: current <— first node in OPEN, remove from OPEN 

6: if current is in the goal set then 

7: return SUCCESS and the path to current 

8: end if 

9: if current is not in a previously occupied C-space grid cell then 

10: mark grid cell occupied 

11: counter counter + 1 

12: for each control in the discrete control set do 

13: integrate control forward a short time At from current to g new 

14: if the path to q new is collision-free then 

15: compute cost of the path to q new 

16: place <7 n ew in OPEN, sorted by cost 

17: parent [q n ew] t— current 

18: end if 

19: end for 

20: end if 

21: end while 
22: return FAILURE 


Integration of the controls does not move the mobile robot to exact grid 
points. Instead, the C-space grid comes into play in lines 9 and 10. When a 
node is expanded, the grid cell it sits in is marked “occupied.” In future, any 
node in this occupied cell will be pruned from the search. This prevents the 
search from expanding nodes that are close by nodes reached with a lower cost. 

No more than MAXCOUNT nodes, where MAXCOUNT is a value chosen by the 
user, are considered during the search. 

The time At should be chosen small enough that each motion step is “small.” 
The size of the grid cells should be chosen as large as possible while ensuring 
that integration of any control for time At will move the mobile robot outside 
of its current grid cell. 

The planner terminates when current lies inside the goal region, when there 
are no more nodes left to expand (perhaps due to obstacles), or when MAXCOUNT 
nodes have been considered. Any path found is optimal for the choice of cost 
function and other parameters to the problem. The planner actually runs faster 
in somewhat cluttered spaces, as the obstacles help guide the exploration. 

Some examples of motion plans for a car are shown in Figure 10.15. 
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Figure 10.15: (Left) A minimum-cost path for a car-like robot where each ac¬ 
tion has identical cost, favoring a short path. (Right) A minimum-cost path 
where reversals are penalized. Penalizing reversals requires a modification to 
Algorithm 2. 

10.4.2.2 Grid-Based Motion Planning for a Robot Arm 

One method for planning the motion for a robot arm is to decouple the problem 
into a path planning problem followed by a time-scaling of the path: 

(i) Apply a grid-based or other path planner to find an obstacle-free path in 
C-space. 

(ii) Time scale the path to find the fastest trajectory along the path that 
respects the robot’s dynamics, as described in Chapter 9.4. Or use any 
less aggressive time scaling. 

Since the motion planning problem is broken into two steps (path planning plus 
time scaling), the resultant motion will not be time-optimal in general. 

Another approach is to plan directly in the state space. Given a state (q, q) 
of the robot arm, let A(q, q ) represent the set of feasible accelerations based on 
the limited joint torques. To discretize the controls, the set A(q, q) is intersected 
with a grid of points of the form 


n 

y. c -i a iG- 

i =1 

where Cj is an integer, a* > 0 is the acceleration step size in the (/,; direction, 
and e,; is a unit vector in the Ah direction (Figure 10.16). 

As the robot moves, the acceleration set A(q, q) changes, but the grid remains 
fixed. Because of this, and assuming a fixed integration time At at each “step” 
in a motion plan, the reachable states of the robot (after any integral number of 
steps) are confined to a grid in state space. To see this, consider a single joint 
angle of the robot, q\, and assume for simplicity zero initial velocity <ji(0) = 0. 
The velocity at timestep k takes the form 


qi(k) = qi(k- 1) + a{k)aiAt, 
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Figure 10.16: The instantaneously available acceleration set A(q,q) for a two- 
joint robot, intersected with a grid spaced at ai in cji and 02 in q 2l gives the 
discretized control actions shown in bold. 


where ci(fc) is chosen from a finite set of integers. By induction, the velocity at 
any timestep must be of the form a\k v At, where k v is an integer. The position 
at timestep k takes the form 

qi(k) = qi(k - 1) + <ji (k- 1)A t + ici(fc)ai(At) 2 . 

Plugging in the form of the velocity, we find that the position at any timestep 
must be of the form a\k p {At) 2 /2 + <71 (0), where k p is an integer. 

To find a trajectory from a start node to a goal set, a breadth-first search 
can be employed to create a search tree on the state space nodes. When a node 
(q, q) in the state space is explored from, the set A(q, q) is evaluated to find the 
discrete set of control actions. New nodes are created by integrating the control 
actions for time At. A node is discarded if the path to it is in collision or if it 
has been reached previously (i.e., by a trajectory taking the same or less time). 

Because the joint angles and angular velocities are bounded, the state space 
grid is finite, and therefore it can be searched in finite time. The planner 
is resolution complete and returns a time-optimal trajectory, subject to the 
resolution specified in the control grid and timestep At. 

The control grid step sizes must be chosen small enough that A(q,q ), for 
any feasible state ( q,q ), contains a representative set of points of the control 
grid. Choosing a finer grid for the controls, or a smaller timestep At, creates a 
finer grid in the state space and a higher likelihood of finding a solution amidst 
obstacles. It also allows choosing a smaller goal set while keeping points of the 
state space grid inside the set. 

Finer discretization comes at a computational cost, however. If the resolu¬ 
tion of the control discretization is increased by a factor of r in each dimension 
(i.e., each a; is reduced to aj/r), and the timestep size is divided by a factor of 
r, the computation time spent growing the search tree for a given robot motion 
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duration increases by a factor of r raT , where n is the number of joints. For ex¬ 
ample, increasing the control grid resolution by a factor of r = 2 and decreasing 
the timestep by a factor of r = 4 for a three-joint robot results in a search that 
is likely to take 2 3 * 4 = 4096 times longer to complete. The high computational 
complexity of the planner makes it impractical beyond a few degrees of freedom. 

The description above ignores one important issue: the feasible control set 
A(q, q ) changes during a timestep, so the control chosen at the beginning of 
the timestep may no longer be feasible by the end of the timestep. For that 
reason, a conservative approximation A(q , q) C A(q, q ) should be used instead. 
This set should remain feasible over the duration of a timestep regardless of 
which control action is chosen. How to determine a conservative approximation 
A(q, q) is beyond the scope of this chapter, but it has to do with bounds on how 
rapidly the arm’s mass matrix M(q) changes with q and how fast the robot is 
moving. At low speeds q and short durations At, the conservative set A(q, q) is 
very close to A(q, q). 

10.5 Sampling Methods 

Each of the grid-based methods discussed above delivers optimal solutions sub¬ 
ject to the chosen discretization. A drawback of the approaches is their high 
computational complexity, making them unsuitable for systems of more than a 
few degrees of freedom. 

A different class of planners, known as sampling methods, relies on a random 
or deterministic function to choose a sample from the C-space or state space; 
a function to evaluate whether a sample or motion is in Af ree ; a function to 
determine nearby previous free-space samples; and a simple local planner to try 
to connect to, or move toward, the new sample. These functions are used to 
build up a graph or tree representing feasible motions of the robot. 

Sampling methods generally give up on the resolution-optimal solutions of 
a grid search in exchange for the ability to find satisficing solutions quickly in 
high-dimensional state spaces. The samples are chosen to form a roadmap or 
search tree that quickly approximates the free space Xf Tee using fewer samples 
than would typically be required by a fixed high-resolution grid, where the 
number of grid points increases exponentially with the dimension of the search 
space. Most sampling methods are probabilistically complete: the probability of 
finding a solution, when one exists, approaches 100% as the number of samples 
goes to infinity. 

Two major classes of sampling methods are rapidly-exploring random trees 
(RRTs) and probabilistic roadmaps (PRMs). RRTs use a tree representation for 
single-query planning in either C-space or state space, while PRMs are primarily 
C-space planners that create a roadmap graph for multiple-query planning. 
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10.5.1 The RRT 

The RRT algorithm searches for a collision-free motion from an initial state 
£start to a goal set A goa i. It applies to kinematic problems, where the state 
x is simply the configuration q , as well as dynamic problems, where the state 
includes the velocity. The basic RRT grows a single tree from x s tart as outlined 
in Algorithm 3. 


Algorithm 3 RRT algorithm. 

1: initialize search tree T with x s t a rt 

2: while T is less than the maximum tree size do 

3: a; S amp <— sample from X 

4: ^nearest <- nearest node in T to s sam p 

5: employ a local planner to find a motion from a.’ near est to a; new in 

the direction of x samp 
6: if the motion is collision-free then 

7: add x new to T with an edge from ^nearest to a; new 

8 : if £new is in X goa \ then 

9: return SUCCESS and the motion to x new 

10: end if 

11: end if 

12: end while 
13: return FAILURE 


In a typical implementation for a kinematic problem (where x is simply q ), 
the sampler in line 3 chooses x samp randomly from an almost-uniform distribu¬ 
tion over A, with a slight bias toward states in A goa i. The closest node x nea rest in 
T (line 4) is the one minimizing the Euclidean distance to x samp . The state x ne w 
(line 5) is chosen as the state a small distance d from Nearest on the straight line 
to £ sam p. Because d is small, a very simple local planner, e.g., one that returns 
a straight line motion, will often find a motion connecting x near est to x new . If 
the motion is collision-free, the new state x new is added to T. 

The net effect is that the nearly uniformly distributed samples “pull” the 
tree toward them, causing the tree to rapidly explore Af ree . An example of the 
effect of this pulling action on exploration is shown in Figure 10.17. 

The basic algorithm leaves many choices: how to sample from X (line 3), 
how to define the “nearest” node in T (line 4), and how to plan the motion 
to make progress toward x samp (line 5). Even a small change to the sampling 
method, for example, can yield a dramatic change in the running time of the 
planner. A wide variety of planners have been proposed in the literature based 
on these choices and other variations. Some of these variations are described 
below. 
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Figure 10.17: (Left) A tree generated by applying a uniformly-distributed ran¬ 
dom motion from a randomly chosen tree node results in a tree that does not 
explore very far. (Right) A tree generated by the RRT algorithm using sam¬ 
ples drawn randomly from a uniform distribution. Both trees have 2000 nodes. 
Figure from [60] used with permission. 

10.5.1.1 Line 3: The Sampler 

The most obvious sampler is one that samples randomly from a uniform distri¬ 
bution over X. This is straightforward for Euclidean C-spaces R“; for n-joint 
robot C-spaces T n = S 1 x ... x S 1 (n times), where we can choose a uniform 
distribution over each joint angle; and for the C-space R 2 x S' 1 for a mobile 
robot in the plane, where we can choose a uniform distribution over R 2 and 
S 1 individually. The notion of a uniform distribution on some other curved 
C-spaces, for example SO( 3), is less straightforward. 

For dynamic systems, a uniform distribution over the state space can be de¬ 
fined as the cross-product of a uniform distribution over C-space and a uniform 
distribution over a bounded velocity set. 

Although the name “rapidly-exploring random trees” gets its name from 
the idea of a random sampling strategy, the samples need not be generated 
randomly. For example, a deterministic sampling scheme that generates a pro¬ 
gressively finer (multi-resolution) grid on X could be employed instead. To 
reflect this more general view, the approach has been called rapidly-exploring 
dense trees (RDTs), emphasizing the key point that the samples should even¬ 
tually become dense in the state space (i.e., as the number of samples goes to 
infinity, the samples become arbitrarily close to every point in X). 

10.5.1.2 Line 4: Defining the Nearest Node 

Finding the “nearest” node depends on a definition of distance on X. For an 
unconstrained kinematic robot on C = R", a natural choice for the distance 
between two points is simply the Euclidean distance. For other spaces, the 
choice is less obvious. 

As an example, for a car-like robot with a C-space R 2 x S 1 , which configu¬ 
ration is closest to the configuration £ sa m P : one that is rotated twenty degrees 
relative to x samp , one that is 2 meters straight behind it, or one that is 1 meter 
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Figure 10.18: Which of the three dashed configurations of the car is “closest” 
to the configuration in gray? 


straight to the side of it (Figure 10.18)? Since the motion constraints prevent 
spinning in place or moving directly sideways, the configuration that is 2 meters 
straight behind is best positioned to make progress toward x samp . Thus defining 
a notion of distance requires 

• combining components of different units (e.g., degrees, meters, degrees/second, 
meters/second) into a single distance measure; and 

• taking into account the motion constraints of the robot. 

The closest node x ne arest should perhaps be defined as the one that can reach 
x samp the fastest, but computing this is as hard as solving the motion planning 
problem. 

A simple choice of a distance measure from x to x S amp is the weighted sum of 
the distances along the different components of x samp — x. The weights choose 
the relative importance of the different components. If more is known about 
the time-limited reachable sets of a motion-constrained robot from a state x, 
this information can be used in determining the nearest node. In any case, 
the nearest node should be computed quickly. Finding a nearest neighbor is a 
common problem in computational geometry, and various algorithms, such as 
fed trees and hashing, can be used to solve it efficiently. 

10.5.1.3 Line 5: The Local Planner 

The job of the local planner is to find a motion from x ne arest to some point 
Xnew which is closer to x samp . The planner should be simple and it should run 
quickly. Three examples are: 

Straight-line planner. The plan is a straight line to x new , which may be 
chosen at x samp or at a fixed distance d from x ne arest on the straight line 
to x sam p. This is for kinematic systems with no motion constraints. 

Discretized controls. For systems with motion constraints, such as wheeled 
mobile robots or dynamic systems, the controls can be discretized into a 
discrete set {iq,U2 ,...}, as in the grid methods with motion constraints 
(Section 10.4.2 and Figures 10.14 and 10.16). Each control is integrated 
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from ^nearest for a fixed time At using x = f(x,u). The resulting state 
that is closest to x samp is chosen as x new . 

Wheeled robot planners. For a wheeled mobile robot, local plans can be 
found using Reeds-Shepp curves or polynomial functions of time of the 
differentially flat output, as described in Chapter 13.3.3. 

Other robot-specific local planners can be designed. 

10.5.1.4 Other RRT Variants 

The performance of the basic RRT algorithm depends heavily on the choice 
of the sampling method, the distance measure, and the local planner. Beyond 
these choices, two other variants of the basic RRT are outlined below. 

Bidirectional RRT. The bidirectional RRT grows two trees: one “forward” 
from :r s tart and one “backward” from a; goa i. The algorithm alternates between 
growing the forward tree and the backward tree, and every so often attempts 
to connect the two trees by choosing x samp from the other tree. The advantage 
of this approach is that a single goal state x goa i can be reached exactly, rather 
than just a goal set ff goa i. Another advantage is that in many environments, 
the two trees are likely to find each other much faster than a single “forward” 
tree will find a goal set. 

The major problem is that the local planner might not be able to connect 
the two trees exactly. For example, the discretized controls planner of Sec¬ 
tion 10.5.1.3 is highly unlikely to create a motion exactly to a node in the other 
tree. In this case, the two trees may be considered more-or-less connected when 
points on each tree are sufficiently close. The “broken” discontinuous trajectory 
can be returned and patched by a smoothing method (Section 10.8). 

RRT*. The basic RRT algorithm returns SUCCESS once a motion to T goa i 
is found. An alternative is to continue running the algorithm and to terminate 
the search only when another termination condition is reached (e.g., maximum 
running time or maximum tree size). Then the motion with the minimum cost 
can be returned. In this way, the RRT solution may continue to improve as 
time goes by. Because edges in the tree are never deleted or changed, however, 
the RRT generally does not converge to an optimal solution. 

RRT* is a variation of the single-tree RRT that continually rewires the search 
tree to ensure that it always encodes the shortest path from x start to each node 
in the tree. The basic approach works for C-space path planning with no motion 
constraints, allowing exact paths from any node to any other node. 

To modify the RRT to the RRT*, line 7 of the RRT algorithm, which inserts 
a: new in T with an edge from Nearest to a; ne w, is replaced by a test of all nodes 
x £ Aji ear in T that are sufficiently near to x nevr . An edge to x new is created 
from the x £ T n e ar that (1) has a collision-free motion by the local planner and 
(2) minimizes the total cost of the path from x start to a; new , not just the cost of 
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Figure 10.19: (Left) The tree generated by an RRT after 20,000 nodes. The goal 
region is the square at the top right corner, and the shortest path is indicated. 
(Right) The tree generated by RRT* after 20,000 nodes. Figure from [47] used 
with permission. 


the added edge. The total cost is the cost to reach the candidate x £ <T near plus 
the cost of the new edge. 

The next step is to consider each x £ X neal to see if it could be reached by 
lower cost by a motion through x nevr . If so, the parent of x is changed to x nevr . 
In this way, the tree is incrementally rewired to eliminate high-cost motions in 
favor of the minimum-cost motions available so far. 

The definition of X neaT depends on the number of samples in the tree, details 
of the sampling method, the dimension of the search space, and other factors. 

Unlike the RRT, the solution provided by RRT* approaches the optimal 
solution as the number of sample nodes increases. Like the RRT, RRT* is 
probabilistically complete. Figure 10.19 demonstrates the rewiring behavior of 
RRT* compared to RRT for a simple example in C = K 2 . 

10.5.2 The PRM 

The PRM uses sampling to build a roadmap representation of Cf ree (Section 10.3) 
before answering any specific queries. The roadmap is an undirected graph: the 
robot can move in either direction along any edge exactly from one node to the 
next. For this reason, PRMs primarily apply to kinematic problems for which 
an exact local planner exists that can find a path (ignoring obstacles) from any 
qi to any q 2 . The simplest example is a straight-line planner for a robot with 
no kinematic constraints. 

Once the roadmap is built, a particular start node q star t can be added to 
the graph by attempting to connect it to the roadmap, starting with the closest 
node. The same is done for the goal node q goa i. The graph is then searched for 
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Figure 10.20: An example PRM roadmap for a point robot in C = R 2 . The 
k = 3 closest neighbors are considered for connection to a sample node q. The 
degree of a node can be greater than three since it may be a close neighbor of 
many nodes. Figure from [19] used with permission. 


a path, typically using A*. Thus the query can be answered efficiently once the 
roadmap has been built. 

PRMs allow the possibility of building a roadmap quickly and efficiently 
relative to constructing a roadmap using a high-resolution grid representation. 
This is because the volume fraction of the C-space that is “visible” by the local 
planner from a given configuration does not typically decrease exponentially 
with increasing dimension of the C-space. 

The algorithm to construct a roadmap R with N nodes is outlined in Algo¬ 
rithm 4 and illustrated in Figure 10.20. 

Algorithm 4 PRM roadmap construction algorithm (undirected graph). 

1: for i = 1 ... N do 

2: qt -s— sample from Cf ree 

3: add qi to R 

4: end for 
5: for * = 1.. . N do 

6: J\T(qi) <— k closest neighbors of qi 

7: for each q £ M(q{) do 

8: if there is a collision-free local path from q to qi and 

there is not already an edge from q to qi then 
9: add an edge from q to qi to the roadmap R 

10: end if 

11: end for 

12: end for 
13: return R 
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A key choice in the PRM roadmap construction algorithm is how to sample 
from Cf ree - While the default might be sampling randomly from a uniform 
distribution on C and eliminating configurations in collision, it has been shown 
that sampling more densely near obstacles can improve the likelihood of finding 
narrow passages, thus significantly reducing the number of samples needed to 
properly represent the connectivity of Cf ree - Another option is deterministic 
multi-resolution sampling. 

10.6 Virtual Potential Fields 

Virtual potential field methods are inspired by potential energy fields in nature, 
such as gravitational and magnetic fields. From physics we know that a potential 
field V(q) defined over C induces a force F = — dV/dq that drives an object from 
high to low potential. For example, if h is the height above the Earth’s surface in 
a uniform gravitational potential field (g = 9.81 m/s 2 ), then the potential energy 
of a mass m is V(h) = mgh and the force acting on it is F = —dV/dh = —mg. 
The force will cause the mass to fall to the Earth’s surface. 

In robot motion control, the goal configuration q gos d is assigned a low virtual 
potential energy and obstacles are assigned a high virtual potential. Applying a 
force to the robot proportional to the negative gradient of the virtual potential 
naturally pushes the robot toward the goal and away from the obstacles. 

A virtual potential field is very different from the planners we have seen so 
far. Typically the gradient of the field can be calculated quickly, so the motion 
can be calculated in real time (reactive control) instead of planned in advance. 
With appropriate sensors, the method can even handle obstacles that move or 
appear unexpectedly. The drawback of the basic method is that the robot can 
get stuck in local minima of the potential field, away from the goal, even when 
a feasible motion to the goal exists. In certain cases it is possible to design the 
potential to guarantee that the only local minimum is at the goal, eliminating 
this problem. 

10.6.1 A Point in C-space 

Let’s begin by assuming a point robot in its C-space. A goal configuration q g0 a\ 
is typically encoded by a quadratic potential energy “bowl” with zero energy at 
the goal, 

T'goal (q) = 2 (? — Qgoal) A (<7 — (/goal): 

where K is a symmetric positive-definite weighting matrix (for example, the 
identity matrix). The force induced by this potential is 


F g osd(q) = = ^( 9 goai - q), 


an attractive force proportional to the distance from the goal. 
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The repulsive force induced by a C-obstacle B can be calculated from the 
distance d(q,B) to the obstacle (Section 10.2.2): 


V B {q) 


k 

2<P(q,BY 


(10.3) 


where k > 0 is a scaling factor. The potential is only properly defined for points 
outside the obstacle, d(q, B) > 0. The force induced by the obstacle potential is 


Fb 0 ?) 


dV B k dd 
HhY ~ d 3 (q,B)d~q 


The total potential is obtained by summing the attractive goal potential and 
the repulsive obstacle potentials, 


F{q) = ^goai (q) + Y^' p Bi (q), 

i 


yielding a total force 


F{q) = Fgoai{q) + Y F bM)- 

i 


Note that the sum of the attractive and repulsive potentials may not give a 
minimum (zero force) exactly at g goa i. Also, it is common to put a bound 
on the maximum potential and force, as the simple obstacle potential (10.3) 
would otherwise yield unbounded potentials and forces near the boundaries of 
obstacles. 

Figure 10.21 shows a potential field for a point in R 2 with three circular 
obstacles. The contour plot of the potential field clearly shows the global min¬ 
imum near the center of the space (near the goal marked with a +), a local 
minimum near the two obstacles on the left, as well as saddles (critical points 
that are a maximum in one direction and a minimum in the other direction) 
near the obstacles. Saddles are generally not a problem, as a small perturbation 
allows continued progress toward the goal. Local minima away from the goal 
are a problem, however, as they attract nearby starting points. 

To actually control the robot using the calculated F(q), we have several 
options, two of which are: 

• Apply the calculated force plus damping, 


u = F(q) + Bq. 


(10.4) 


If B is positive definite, then it dissipates energy for all g / 0, reducing 
oscillation and guaranteeing that the robot will come to rest. If B = 0, the 
robot continues to move while maintaining constant total energy, which 
is the sum of the initial kinetic energy | q T (0)M(q(0))q(0) plus the initial 
virtual potential energy V(q( 0)). 

The motion of the robot under the control law (10.4) can be visualized as 
a ball rolling in gravity on the potential surface of Figure 10.21, where the 
dissipative force is rolling friction. 
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Figure 10.21: (Top left) Three obstacles and a goal point, marked with a +, 
in R 2 . (Top right) The potential function summing the bowl-shaped potential 
pulling the robot to the goal with the repulsive potentials of the three obstacles. 
The potential function saturates at a specified maximum value. (Bottom left) 
A contour plot of the potential function, showing the global minimum, a local 
minimum, and four saddles: between each obstacle and the boundary of the 
workspace, and between the two small obstacles. (Bottom right) Forces induced 
by the potential function. 


• Treat the calculated force as a commanded velocity instead: 

q = F(q). (10.5) 

This automatically eliminates oscillations. 

Using the simple obstacle potential (10.3), even distant obstacles have a 
nonzero effect on the motion of the robot. To speed up evaluation of the repul¬ 
sive terms, distant obstacles could be ignored. We can define a range of influence 
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of the obstacles change > 0 so that the potential is zero for all d(q,B) > d range : 



Another issue is that d(q, B ) and its gradient are generally difficult to calcu¬ 
late. An approach to dealing with this is described in Section 10.6.3. 

10.6.2 Navigation Functions 

A significant problem with the potential field method is local minima. While 
potential fields may be appropriate for relatively uncluttered spaces, or for rapid 
response to unexpected obstacles, they are likely to get the robot stuck in local 
minima for many practical applications. 

One method that avoids this issue is the wavefront planner of Figure 10.11. 
The wavefront algorithm creates a local-minimum-free potential function by 
a breadth-first traversal of every cell reachable from the goal cell in a grid 
representation of the free space. Therefore, if a solution exists to the motion 
planning problem, then simply moving “downhill” at every step is guaranteed 
to bring the robot to the goal. 

Another approach to local-minimum-free gradient following is based on re¬ 
placing the virtual potential function with a navigation function. A naviga¬ 
tion function ip(q) is a type of virtual potential function that 

(i) is smooth (or at least twice differentiable) on q ; 

(ii) has a bounded maximum value (e.g., 1) on the boundaries of all obstacles; 

(iii) has a single minimum at g goa i; and 

(iv) has a full-rank Hessian d 2 ip/dq 2 at all critical points q where dip/dq = 0 
(i.e., tp(q) is a Morse function). 

Condition 1 ensures that the Hessian d 2 ip/dq 2 exists. Condition 2 puts an upper 
bound on the virtual potential energy of the robot. The key conditions are 3 
and 4. Condition 3 ensures that of the critical points of <p(q) (including minima, 
maxima, and saddles), there is only one minimum, at ^goai- This ensures that 
(/goal is at least locally attractive. However, there may be saddle points which are 
minima along a subset of directions. Condition 4 ensures that the set of initial 
states that are attracted to any saddle point has empty interior (zero measure), 
and therefore almost every initial state converges to the unique minimum g goa i. 

While constructing navigation potential functions with only a single min¬ 
imum is non-trivial, Rimon and Koditschek show how to construct them for 
the particular case of an n-dimensional Cf ree consisting of all points inside an 
n-sphere of radius R and outside of smaller spherical obstacles Bi of radius r, 
centered at q t , i.e., {q £ K” | ||(/|| < R and ||g — (/,;|| > r$ for all i}. This is called 
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Figure 10.22: (Left) A model “sphere world” with five circular obstacles. The 
contour plot of a navigation function is shown. The goal is at (0,0). Note 
that the obstacles induce saddle points near the obstacles, but no local minima. 
(Right) A “star world” obtained by deforming the obstacles and the potential 
while retaining a navigation function. Figure from [109] used with permission. 


a sphere world. While a real C-space is unlikely to be a sphere world, Rimon 
and Koditschek show that the boundaries of the obstacles, and the associated 
navigation function, can be deformed to a much broader class of star-shaped ob¬ 
stacles. A star-shaped obstacle is one that has a center point from which the line 
segment to any point on the obstacle boundary is contained completely within 
the obstacle. A star world is a star-shaped C-space which has star-shaped ob¬ 
stacles. Thus finding a navigation function for an arbitrary star world reduces 
to finding a navigation function for a “model” sphere world that has centers 
at the centers of the star-shaped obstacles, then stretching and deforming that 
navigation function to one that fits the star world. Rimon and Koditschek give 
a systematic procedure to accomplish this. 

Figure 10.22 shows a deformation of a navigation function on a model sphere 
world to a star world for the case C CR 2 . 

10.6.3 Workspace Potential 

A difficulty in calculating the repulsive force from an obstacle is calculating the 
distance to the obstacle, d(q , B). One approach that avoids the exact calculation 
is to represent the boundary of an obstacle as a set of point obstacles, and to 
represent the robot by a small set of control points. Let the Cartesian location 
of control point i on the robot be written /) (g) £ R 3 and boundary point j of the 
obstacle be Cj £ R 3 . Then the distance between the two points is ||/,(g) — Cj\\, 
and the potential at the control point i due to the obstacle point j is 

Kiq) = 


2mq)-c 3 \?' 
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yielding the repulsive force at the control point 


dV 

F'M = - 


dq \\fi(q) -Cj\\ 4 


|| ) (/*(<?) - cj) e 


To turn the linear force FO(q) £ R 3 into a generalized force Fij(q) £ R" 
acting on the robot arm or mobile robot, we first find the Jacobian Ji(q) £ R 3xn 
relating q to the linear velocity of the control point /,: 

i dfi . 

ji = -R-q = Ji(q)q- 


By the principle of virtual work, the generalized force Fij(q) £ R" due to the 
repulsive linear force FO (q) £ R 3 is simply 

FiM = J? {q)F' ij {q). 

Now the total force F{q) acting on the robot is the sum of the easily calculated 
attractive force F goa \(q) and the repulsive forces Fij(q) for all i and j. 


10.6.4 Wheeled Mobile Robots 

The preceding analysis assumes that a control force u = F(q) + Bq (for control 
law (10.4)) or a velocity q = F(q) (for control law (10.5)) can be applied in any 
direction. If the robot is a wheeled mobile robot subject to rolling constraints 
A{q)q = 0, however, the calculated F(q) must be projected to controls F pro j(g) 
that move the robot tangent to the constraints. For a kinematic robot employing 
the control law q = F pro j(q), a suitable projection is 

Fproj(g) = (i - A T (q)(A(q)A T (q))~ 1 A(q)^F(q). 

For a dynamic robot employing the control law u = F plo j(q)+Bq, the projection 
is discussed in Chapter 8.7. 


10.6.5 Use of Potential Fields in Planners 

A potential field can be used in conjunction with a path planner. For example, 
a best-first search like A* can use the potential as an estimate of the cost-to-go. 
Incorporating search prevents the planner from getting permanently stuck in 
local minima. 


10.7 Nonlinear Optimization 

The motion planning problem can be expressed as a general nonlinear optimiza¬ 
tion with equality and inequality constraints, taking advantage of a number of 
software packages to solve such problems. Nonlinear optimization problems can 
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be solved by gradient-based methods, like sequential quadratic programming 
(SQP), and non-gradient methods, like simulated annealing, Nelder-Mead op¬ 
timization, and genetic programming. Like many nonlinear optimization prob¬ 
lems, however, these methods are not generally guaranteed to find a feasible 
solution when one exists, let alone an optimal one. For methods that use gradi¬ 
ents of the objective function and constraints, however, we can expect a locally 
optimal solution if we start the process with a guess that is “close” to a solution. 

The general problem can be written 


find 

u{t),q(t),T 


(10.6) 

minimizing 

J(u(t),q(t),T) 


(10.7) 

subject to 

x(t) = f(x{t),u(t)) 

Vi G [0, T] 

(10.8) 


u(t) G U 

Vi G [0, T\ 

(10.9) 


q(t) G Cf r ee 

Vi G [0, T\ 

(10.10) 


= *^start 


(10.11) 


x(T^) 3? goal* 


(10.12) 


To approximately solve this problem by nonlinear optimization, the con¬ 
trol u(t), trajectory q(t), and equality and inequality constraints (10.8)—(10.12) 
must be discretized. This is typically done by ensuring that the constraints are 
satisfied at a fixed number of points distributed evenly over the interval [0,T], 
and by choosing a finite-parameter representation of the position and/or control 
histories. We have at least three choices of how to parametrize the position and 
controls: 

(i) Parametrize the trajectory q(t). In this case, we solve for the parametrized 
trajectory directly. The control forces u(t) at any time are calculated using 
the equations of motion. This approach does not apply to systems with 
fewer controls than configuration variables, m < n. 

(ii) Parametrize the control u{t). We solve for u(t) directly. Calculating the 
state x(t) requires integrating the equations of motion. 

(iii) Parametrize both q(t ) and u{t). We have a larger number of variables, since 
we are parametrizing both q(t) and u(t). Also, we have a larger number of 
constraints, as q(t) and u must satisfy the dynamic equations x = f(x,u) 
explicitly, typically at a fixed number of points distributed evenly over the 
interval [0, T]. We must be careful to choose the parametrizations of q(t) 
and u(t) to be consistent with each other, so that the dynamic equations 
can be satisfied at these points. 

A trajectory or control history can be parametrized in any number of ways. 
The parameters can be the coefficients of a polynomial in time, the coefficients 
of a truncated Fourier series, spline coefficients, wavelet coefficients, piecewise 
constant acceleration or force segments, etc. For example, the control Ui(t) 
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could be represented by p + 1 coefficients a,j of a polynomial in time: 


p 


Ui(t) = ^2ajt J . 

3 = 0 


In addition to the parameters for the state or control history, the total time 
T may be another control parameter. The choice of parametrization has impli¬ 
cations for the efficiency of the calculation of q(t) and u(t) at a given time t. 
The choice of parametrization also determines the sensitivity of the state and 
control to the parameters, and whether each parameter affects the profiles at all 
times [0, T] or just on a finite-time support base. These are important factors 
in the stability and efficiency of the numerical optimization. 


10.8 Smoothing 


The axis-aligned motions of a grid planner and the randomized motions of sam¬ 
pling planners may lead to jerky motion of the robot. One approach to dealing 
with this issue is to let the planner handle the work of searching globally for a 
solution, then post-process the resulting motion to make it smoother. 

There are many ways to do this; two possibilities are outlined below. 

Nonlinear optimization. While gradient-based nonlinear optimization may 
fail to find a solution if initialized with a random initial trajectory, it can make 
an effective post-processing step, since the plan initializes the optimization with 
a “reasonable” solution. The initial motion must be converted to a parametrized 
representation of the controls, and the cost J(u(t),q(t),T) can be expressed as 
a function of u(t) or q(t). For example, 



penalizes the rate of control change. This has an analogy in human motor 
control, where the smoothness of human arm motions has been attributed to 
minimizing the rate of change of acceleration of the joints. 

Subdivide and reconnect. A local planner can be used to attempt a con¬ 
nection between two distant points on a path. If this new connection is collision- 
free, it replaces the original path segment. Since the local planner is designed 
to produce short, smooth paths, the new path is likely shorter and smoother 
than the original. This test-and-replace procedure can be applied iteratively to 
randomly chosen points on the path. Another possibility is to use a recursive 
procedure that subdivides the path first into two pieces and attempts to replace 
each piece with a shorter path; then, if either portion could not be replaced by 
a shorter path, subdivide again; etc. 
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10.9 Summary 

• A fairly general statement of the motion planning problem is: Given an 
initial state a;(0) = 2 sta rt and a desired final state a: g oai, find a time T and 
a set of controls u : [0, T] —y U such that the motion satisfies x(T ) £ X goa ,\ 
and q(x(t)) £ Cf ree for all t £ [0, T]. 

• Motion planning problems can be classified in the following categories: 
path planning vs. motion planning; fully actuated vs. constrained or un¬ 
deractuated; online vs. offline; optimal vs. satisficing; exact vs. approxi¬ 
mate; with or without obstacles. 

• Motion planners can be characterized by the following properties: multiple- 
query vs. single-query; anytime planning or not; complete, resolution com¬ 
plete, probabilistically complete, or none of the above; and computational 
complexity. 

• Obstacles partition the C-space into free C-space Cf ree and obstacle space 
C 0 bs? C = Cf re e U Cobs- Obstacles may split Cf ree into multiple connected 
components. There is no feasible path between configurations in different 
connected components. 

• A conservative check of whether a configuration q is in collision uses a sim¬ 
plified “grown” representation of the robot and obstacles. If there is no 
collision between the grown bodies, then the configuration is guaranteed 
collision-free. Checking if a path is collision-free usually involves sampling 
the path at finely spaced points, and ensuring that if the individual con¬ 
figurations are collision-free, then the swept volume of the robot path is 
collision-free. 

• The C-space geometry is often represented by a graph consisting of nodes 
and edges between the nodes, where edges represent feasible paths. The 
graph can be undirected (edges flow both directions) or directed (edges 
only flow one direction). Edges can be unweighted or weighted according 
to their cost of traversal. A tree is a directed graph with no cycles in 
which each node has at most one parent. 

• A roadmap path planner uses a graph representation of Cf ree , and path 
planning problems can be solved using a simple path from g sta rt onto the 
roadmap, a path along the roadmap, and a simple path from the roadmap 

tO *7goal ■ 

• A* is a popular search method that finds minimum-cost paths on a graph. 
It operates by always exploring from the node that is (1) unexplored and 
(2) expected to be on a path with minimum estimated total cost. The 
estimated total cost is the sum of edge weights to reach the node from the 
start node plus an estimate of the cost-to-go to the goal. The cost-to-go 
estimate should be optimistic to ensure that the search returns an optimal 
solution. 


10.9. Summary 


353 


• A grid-based path planner discretizes the C-space into a graph consisting 
of neighboring points on a regular grid. A multi-resolution grid can be used 
to allow large steps in wide-open spaces and smaller steps near obstacle 
boundaries. 

• Discretizing the control set allows robots with motion constraints to take 
advantage of grid-based methods. If integrating a control does not land 
the robot exactly on a grid point, the new state may still be pruned if a 
state in the same grid cell was already achieved with a lower cost. 

• The basic RRT algorithm grows a single search tree from a; s tart to find a 
motion to A goa i. It relies on a sampler to find a sample x samp in A; an 
algorithm to find the closest node a; nea rest hr the search tree; and a local 
planner to find a motion from a; near est to a point closer to a; samp . The 
sampling is chosen to cause the tree to explore Af ree quickly. 

• The bidirectional RRT grows a search tree from both x start and i goa i and 
attempts to join them up. RRT* returns solutions that tend toward the 
optimal as planning time goes to infinity. 

• The PRM builds a roadmap of Cf ree for multiple-query planning. The 
roadmap is built by sampling Cf ree N times, then using a local planner to 
attempt to connect each sample with several of its nearest neighbors. The 
roadmap is searched for plans using A*. 

• Virtual potential fields are inspired by potential energy fields such as grav¬ 
itational and electromagnetic fields. The goal point creates an attractive 
potential while obstacles create a repulsive potential. The total poten¬ 
tial V{q) is the sum of these, and the virtual force applied to the robot 
is F(q) = —dV/dq. The robot is controlled by applying this force plus 
damping, or by simulating first-order dynamics and driving the robot with 
F{q) as a velocity. Potential field methods are conceptually simple but 
may get the robot stuck in local minima away from the goal. 

• A navigation function is a potential function with no local minima. Nav¬ 
igation functions result in near-global convergence to g goa i. While navi¬ 
gation functions are difficult to design in general, they can be designed 
systematically for certain environments. 

• Motion planning problems can be converted to general nonlinear optimiza¬ 
tion problems with equality and inequality constraints. While optimiza¬ 
tion methods can be used to find smooth, near-optimal motions, they tend 
to get stuck in local minima in cluttered C-spaces. Optimization methods 
typically require a good initial guess at a solution. 

• Motions returned by grid-based and sampling-based planners tend to be 
jerky. Smoothing the plan using nonlinear optimization or subdivide-and- 
reconnect can improve the quality of the motion. 
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10.10 Notes and References 

Excellent textbooks covering motion planning broadly include the original text 
by Latombe [57] in 1991 and the more recent texts by Choset et al. [19] and 
LaValle [59]. Other summaries of the state-of-the-art in motion planning can 
be found in the Handbook of Robotics [49], and, particularly for robots subject 
to nonholonomic and actuation constraints, in the Control Handbook [74], the 
Encyclopedia of Systems and Control [73], and the textbook by Murray, Li, and 
Sastry [90]. Search algorithms and other algorithms for artificial intelligence are 
covered in detail by Russell and Norvig [111]. 

Landmark early work on motion planning for Shakey the Robot at SRI led 
to the development of A* search in 1968 by Hart, Nilsson, and Raphael [35]. 
This work built on the newly established approach to dynamic programming 
for optimal decision-making, as described by Bellman and Dreyfus [8], and im¬ 
proved on the performance of Dijkstra’s algorithm [25]. A suboptimal anytime 
variant of A* was proposed in [66]. Early work on multiresolution path plan¬ 
ning is described in [45, 70, 31, 36] based on hierarchical decompositions of 
C-space [112], 

One early line of work focused on exact characterization of the free C-space 
in the presence of obstacles. The visibility graph approach for polygons mov¬ 
ing among polygons was developed by Lozano-Perez and Wesley in 1979 [71]. 

In more general settings, researchers used sophisticated algorithms and math¬ 
ematical methods to develop cellular decompositions and exact roadmaps of 
the free C-space. Important examples of this work are a series of papers by 
Schwartz and Sharir on the piano movers’ problem [113, 114, 115] and Canny’s 
PhD thesis [16]. 

As a result of the mathematical sophistication and high computational com¬ 
plexity needed to exactly represent the topology of C-spaces, a movement formed 
in the 1990s to approximately represent C-space using samples, and that move¬ 
ment carries on strong today. That line of work has followed two main branches, 
probabilistic roadmaps (PRMs) [48] and rapidly exploring random trees (RRTs) [60, 
62, 61]. Due to their ability to handle complex high-dimensional C-spaces rel¬ 
atively efficiently, research in sampling-based planners has exploded, and some 
of the subsequent work is summarized in [19, 59]. The bidirectional RRT and 
RRT*, highlighted in this chapter, are described in [59] and [47], respectively. 

The grid-based approach to motion planning for a wheeled mobile robot was 
introduced by Barraquand and Latombe [6], and the grid-based approach to 
time-optimal motion planning for a robot arm with dynamic constraints was 
introduced in [17, 27, 26]. 

The GJK algorithm for collision detection was derived in [34]. Open-source 
collision-detection packages are implemented in the Open Motion Planning Li¬ 
brary (OMPL) [133] and the Robot Operating System (ROS). An approach to 
approximating polyhedra with spheres for fast collision detection is described 
in [43]. 

The potential field approach to motion planning and real-time obstacle 
avoidance was first introduced by Khatib and is summarized in [51]. A search- 
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based planner using a potential field to guide the search is described by Bar- 
raquand et al. [5]. The construction of navigation functions, potential functions 
with a unique local minimum, is described in a series of papers by Koditschek 
and Rimon [55, 53, 54, 109, 110], 

Nonlinear optimization-based motion planning has been formulated in a 
number of publications, including the classic computer graphics paper by Witkin 
and Kass [140] using optimization to generate the motions of an animated jump¬ 
ing lamp; work on generating motion plans for dynamic nonprehensile manip¬ 
ulation [76]; Newton algorithms for optimal motions of mechanisms [64]; and 
more recent developments in short-burst sequential action control, which solves 
both the motion planning and feedback control problems [2, 136]. Path smooth¬ 
ing for mobile robot paths by subdivide and reconnect is described by Laumond 
et al. [58]. 
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Figure 10.23: Exercise 4. 


10.11 Exercises 


1. A path is homotopic to another if it can be continuously deformed into the 
other without moving the endpoints. In other words, it can be stretched and 
pulled like a rubber band, but it cannot be cut and pasted back together. For 
the C-space of Figure 10.2, draw a path from the start to the goal that is not 
homotopic to the one shown. 

2. Label the connected components in Figure 10.2. For each connected com¬ 
ponent, draw a picture of the robot for one configuration in the connected 
component. 

3. Assume that 02 joint angles in the range [175°, 185°] result in self-collision 
for the robot of Figure 10.2. Draw the new joint limit C-obstacle on top of the 
existing C-obstacles and label the resulting connected components of Cf ree - For 
each connected component, draw a picture of the robot for one configuration in 
the connected component. 

4. Draw the C-obstacle corresponding to the obstacle and translating planar 
robot in Figure 10.23. 

5. Write a program that accepts as input the coordinates of a polygonal robot 
(relative to a reference point on the robot) and the coordinates of a polygo¬ 
nal obstacle and produces as output a drawing of the corresponding C-space 
obstacle. In Mathematica, you may find the function ConvexHull useful. In 
MATLAB, try convhull. 

6. Calculating a square root is typically computationally expensive. For a 
robot and an obstacle represented as collections of spheres (Section 10.2.2), 
provide a method for calculating the distance between the robot and obstacle 
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Figure 10.24: Planning problem for Exercise 8. 


that minimizes the use of square roots. 

7. For Figure 10.8(c), give the order that the nodes in the tree are visited in 
a complete breadth-first search and a complete depth-first search. Assume that 
leftmost branches are always followed first. 

8. Draw the visibility roadmap for the C-obstacles and q s tart and g goa i in Fig¬ 
ure 10.24. Indicate the shortest path. 

9. Not all edges of the visibility roadmap described in Section 10.3 are needed. 
Prove that an edge between two vertices of C-obstacles need not be included in 
the roadmap if either end of the edge does not hit the obstacle tangentially. In 
other words, if the edge ends by “colliding” with an obstacle, it will never be 
used in a shortest path. 

10 . You will implement an A* path planner for a point robot in a plane with 
obstacles. The planar region is a 100x100 area. The program will generate a 
graph consisting of N nodes and E edges, where N and E are chosen by the 
user. After generating N randomly chosen nodes, the program should connect 
randomly chosen nodes with edges until E unique edges have been generated. 
The cost associated with each edge is the Euclidean distance between the nodes. 
Finally, the program should display the graph, search the graph using A* for the 
shortest path between nodes 1 and N, and display the shortest path or indicate 
FAILURE if no path exists. The heuristic cost-to-go is the Euclidean distance 
to the goal. 

11. Modify the A* planner in Exercise 10 to use a heuristic cost-to-go equal 
to ten times the distance to the goal node. Compare the running time to the 
original A* when they are run on the same graphs. (You may need to use large 
graphs to notice any effect.) Are the solutions found with the new heuristic 
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optimal? 

12. Modify the A* algorithm from Exercise 10 to use Dijkstra’s algorithm 
instead. Comment on the relative running time between A* and Dijkstra’s 
algorithm when each is run on the same graphs. 

13. Write a program that accepts the vertices of polygonal obstacles from a 
user, as well as the specification of a 2R robot arm, rooted at (x, y) = (0, 0), with 
link lengths L\ and L 2 . Each link is simply a line segment. Generate the C-space 
obstacles for the robot by sampling the two joint angles at fc-degree intervals 
(e.g., k = 5) and checking for intersection between the line segments and the 
polygon. Plot the obstacles in the workspace, and in the C-space grid use a black 
square or dot for C-obstacles. (Hint: At the core of this program is a subroutine 
to see if two line segments intersect. If the segments’ corresponding infinite lines 
intersect, you can check if this intersection is within the line segments.) 

14. Write an A* grid path planner for the 2R robot with obstacles, and display 
found paths on the C-space. (See Exercise 13 and Figure 10.10.) 

15. Write a program to implement a virtual potential field for a 2R robot in an 
environment with point obstacles. The two links of the robot are line segments, 
and the user specifies the goal configuration of the robot, the start configuration 
of the robot, and the location of the point obstacles in the workspace. Put two 
control points on each link of the robot and transform the workspace potential 
forces to configuration space potential forces. In one workspace figure, draw 
an example environment consisting of a few point obstacles, and the robot at 
its start and goal configurations. In a second C-space figure, plot the potential 
function as a contour plot over (f?i, 6 2 ), and overlay a planned path from a start 
configuration to a goal configuration. The robot uses the kinematic control law 
Q = F(q). 

See if you can create a planning problem that results in convergence to a 
local minimum for some initial arm configurations, but succeeds in finding a 
path to the goal for other initial arm configurations. 


Chapter 11 

Robot Control 


A robot arm can exhibit a number of different behaviors, depending on the task 
and its environment. It can act as a source of programmed motions for tasks 
such as moving an object from one place to another, or tracing a trajectory for 
a spray paint gun. It can act as a source of forces, as when applying a polishing 
wheel to a workpiece. In tasks such as writing on a chalkboard, it must control 
forces in some directions (the force pressing the chalk against the board) and 
motions in others (motion in the plane of the board). When the purpose of the 
robot is to act as a haptic display, rendering a virtual environment, we may 
want it to act like a spring, damper, or mass, yielding in response to forces 
applied to it. 

In each of these cases, it is the job of the robot controller to convert the task 
specification to forces and torques at the actuators. Control strategies to achieve 
the behaviors described above are known as motion control, force control, 
hybrid motion-force control, and impedance control. Which of these 
behaviors is appropriate depends on both the task and the environment. For 
example, a force control goal makes sense when the end-effector is in contact with 
something, but not when it is moving in free space. We also have a fundamental 
constraint imposed by mechanics, irrespective of the environment: the robot 
cannot independently control the motion and force in the same direction. If the 
robot imposes a motion, then dynamics and the environment will determine the 
force, and if the robot imposes a force, then dynamics and the environment will 
determine the motion. 

Once we have chosen a control goal consistent with the task and environ¬ 
ment, we can use feedback control to achieve it. Feedback control uses position, 
velocity, and force sensors to measure the actual behavior of the robot, com¬ 
pare it to the desired behavior, and modulate the control signals sent to the 
actuators. Feedback is used in nearly all robot systems. 

In this chapter we focus on feedback control for motion control, force control, 
hybrid motion-force control, and impedance control. 
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11.1 Control System Overview 

A typical control block diagram is shown in Figure 11.1(a). Sensors are typically 
potentiometers, encoders, or resolvers for joint position/angle sensing; tachome¬ 
ters for joint velocity sensing; strain gauge joint force/torque sensors; and/or 
multi-axis force-torque sensors at the “wrist” between the end of the arm and 
the end-effector. The controller samples the sensors and updates its control 
signals to the actuators at a rate of hundreds to a few thousands of Hz. In most 
robotic applications, higher control update rates are of limited benefit, given 
time constants associated with the dynamics of the robot and environment. In 
our analysis, we ignore the nonzero sampling time and treat controllers as if 
they are implemented in continuous time. 

While tachometers can be used for direct velocity sensing, a common ap¬ 
proach is to use a digital filter to numerically difference position signals at 
successive time steps. A low-pass filter is often used in combination with the 
differencing filter to reduce high-frequency signal content due to the quantiza¬ 
tion of the differenced position signals. 

As discussed in Chapter 8.9, there are a number of different technologies for 
creating mechanical power, transforming the speeds and forces, and transmitting 
to the robot joints. In this chapter, we lump each joint’s amplifier, actuator, and 
transmission together and treat them as a transformer from low-power control 
signals to forces and torques. This assumption, along with the assumption of 
perfect sensors, allows us to simplify the block diagram of Figure 11.1(a) to the 
one shown in Figure 11.1(b), where the controller produces forces and torques 
directly. The rest of this chapter deals with the control algorithms that go inside 
the “controller” box in Figure 11.1(b). 

Real robot systems are subject to flexibility and vibrations in the joints 
and links, backlash at gears and transmissions, actuator saturation limits, and 
limited resolution of the sensors. These raise significant issues in design and 
control, but they are beyond the scope of this chapter. 

11.2 Motion Control 

A motion controller can be defined in joint space or task space. In joint space, 
the controller is fed a steady stream of joint positions 9d(t) and velocities 9d(t) 
to track, and the goal is to drive the robot to track this trajectory. When 
the trajectory is expressed in task space, the controller is fed a steady stream 
of end-effector configurations Xd(t) and velocities Vd[t). If the end-effector 
configuration is represented by a minimum set of coordinates, then Xd(t) £ R m 
and Vd{t) = Xd{t). If the end-effector configuration is represented as an element 
of SE( 3), then Xd(t ) £ SE( 3) and Vd{t) £ R 6 , where Vd = XJ x Xd is the twist 
expressed in the end-effector frame and Vd = XdXj 1 is the twist expressed in 
the space frame. 
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Figure 11.1: (a) A typical robot control system. An inner control loop is used to 
help the amplifier and actuator achieve the desired force or torque. For example, 
a DC motor amplifier in torque control mode may sense the current actually 
flowing through the motor and implement a local controller to better match 
the desired current, since the current is proportional to the torque produced 
by the motor, (b) A simplified model with ideal sensors and a controller block 
that directly produces forces and torques. This assumes ideal behavior of the 
amplifier and actuator blocks in part (a). Not shown are disturbance forces 
that can be injected before the dynamics block, or disturbance forces or motions 
injected after the dynamics block. 

11.2.1 Motion Control of a Multi-Joint Robot with Velocity 
Input 

As discussed in Chapter 8, we typically assume direct control of the forces or 
torques at robot joints, and the robot’s dynamics transform those controls to 
joint accelerations. In relatively rare cases, however, we can assume direct con¬ 
trol of the joint velocities. An example is when the actuators are stepper motors. 
In this case, the velocity of a joint is determined directly by the frequency of 
the pulse train sent to the stepper. 1 Another example is when the amplifier 
for an electric motor is placed in velocity control mode—the amplifier attempts 
to achieve the joint velocity requested by the user, rather than a joint force or 

1 Assuming that the torque requirements are low enough that the stepper motor can keep 
up with the pulse train. 
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torque. 

Under the assumption of perfect velocity control, motion control of a multi¬ 
joint robot would be trivial: at all times, simply command the velocity 

O co -m{t) = 9 d (t), 

where 9 d (t) comes from the desired trajectory. This is called a feedforward or 
open-loop controller, since no feedback (sensor data) is needed to implement 
it. 

In practice, however, position errors will accumulate over time. An alter¬ 
native strategy is to continually measure the actual position of each joint and 
implement a simple feedback controller, 

0 C om (t) = K p (0 d (t ) - 0(t)), 

where K p is a diagonal matrix of positive gains. This controller is called a 
“proportional controller” because it creates a corrective velocity proportional 
to the current position error 9 e (t ) = 0 d (t) — 9(t). This prevents the error from 
growing large over time, which could happen with a purely open-loop controller. 
A drawback, however, is that the robot only moves when there is error. 

We can use feedforward control, which commands motion even when there 
is no error, in combination with feedback control to limit the accumulation of 
error: 

9 com (t ) = 0 d (t) + K p (9 d (t) - 9(t)). (11.1) 

This feedforward-feedback controller is the preferred velocity control law. 

We can write an analogous control law in task space. Let X(t) 6 SE( 3) 
be the configuration of the end-effector as a function of time, and V(t) be the 
end-effector twist expressed in the end-effector frame, i.e., V = X _1 X. The 
task space version of the control law (11.1) is 

V com (t) = V d (t) + K p X e (t), (11.2) 

where X e (t) is not simply X d (t) — X(t), since it does not make sense to subtract 
elements of SE(3). Instead, as we saw in Chapter 6.2, X e should refer to the 
twist which, if followed for unit time, takes X to X d . The matrix representation 
of this twist, expressed in the end-effector frame, is [X e ] = log(X _1 Ad). 2 

If X represents the end-effector configuration using a minimum set of coor¬ 
dinates, then the control law (11.2) can be used with V = X and X e = X d — X. 

The commanded joint velocities 0 com corresponding to V com in Equation (11.2) 
can be calculated using the inverse velocity kinematics from Chapter 6.3, 

^com = J ] (9)V com , 

where J^(Q) is the pseudoinverse of the appropriate Jacobian. 

2 Writing X as X s b (the end-effector frame {b} in a space frame {s}), and Xd as X s d, we 
see that X~^X s d = Xb s X s d = X^d, the configuration Xd expressed in the end-effector frame. 
Therefore log(X — 1 Xd) is the matrix representation of the twist that takes X to Xd in unit 
time, expressed in the end-effector frame. 
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Figure 11.2: A single-joint robot rotating in a gravity field. 

11.2.2 Motion Control of a Single Joint with Torque or Force 
Input 

Stepper-motor-controlled robots are generally limited to applications with low 
or predictable force/torque requirements. Also, robot control engineers do not 
rely on the velocity-control modes of off-the-shelf amplifiers for electric motors, 
because these velocity-control algorithms do not make use of the dynamic model 
of the robot. Instead, robot control engineers use amplifiers in torque-control 
mode: the input to the amplifier is the desired torque (or force). This allows 
the robot control engineer to use a dynamic model of the robot in the design of 
the control law. The ideas are well illustrated by a robot with a single joint, so 
we begin there, then generalize to a multi-joint robot. 

Consider a single motor attached to a single link, as shown in Figure 11.2. 
Let r be the motor’s torque and 9 be the angle of the link. The dynamics can 
be written as 

t = Md + mgr cos 9, (11.3) 

where M is the scalar inertia of the link about the axis of rotation, m is the 
mass of the link, r is the distance from the axis to the center of mass of the link, 
and g > 0 is gravitational acceleration. 

According to the model (11.3), there is no dissipation: if the link is made to 
move and r is then set to zero, the link would move forever. This is unrealistic, 
of course; there is friction at the various bearings, gears, and transmissions. 
Friction modeling is an active research area, but a simple model of rotational 
friction is viscous friction, 

Tfric = b9, (11.4) 

where b > 0. Adding the friction torque, our final model is 

r = M9 + mgr cos 9 + b0, (11.5) 

which we may write more compactly as 

r = MO + h(0,O), (11.6) 

where h contains all terms that depend only on the state, not the acceleration. 
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Figure 11.3: Block diagram of a PID controller. 


For concreteness in the following simulations, we set M = 0.5 kgm 2 , m = 

1 kg, r = 0.1 m, and b = 0.1 Nms/rad. In some examples, the link moves in 
a horizontal plane, so g = 0. In other examples, the link moves in a vertical 
plane, so g = 9.81 m/s 2 . 

11.2.2.1 Feedback Control: PID Control 

The most common feedback control algorithm is linear PID (proportional-integral- 
derivative) control. Defining the error between the desired angle 9 d and actual 
angle 9 as 

e e = 0 d -6, (11.7) 

the PID controller is simply 


t = K p 9 e + Ki 


0 e (t)dt + K d 9 e , 


( 11 . 8 ) 


where the control gains K p , K,. and K d are nonnegative. The proportional gain 
K p acts as a virtual spring that tries to reduce the position error 9 e = 9 d — 6, 
and the derivative gain K d acts as a virtual damper that tries to reduce the 
velocity error 9 e = 9 d — 9. The integral gain, as we will see later, can be used 
to eliminate steady-state errors when the joint is at rest. See the block diagram 
in Figure 11.3. 

For now let’s consider the case where Ki = 0. This is known as PD control. 
(We can similarly define PI, P, I, and D control by setting other gains to zero. 
PD and PI control are the most common variants of PID control.) Let’s also 
assume the robot moves in a horizontal plane, so g = 0. Plugging the control 
law (11.8) into the dynamics (11.5), we get 

M9 + b9 = K p (9 d -9) + K d (9 d - 9). 


( 11 . 9 ) 
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If the goal state is rest at a constant 9 dl then 9 d = 9 d = 0. This is called 
setpoint control. In setpoint control, 9 e = 0 d — 9 1 9 e = —9, and 9 e = —9 1 
Equation (11.9) can be rewritten as the linear mass-spring-damper error dy¬ 
namics 

M9 e + (b + K d )9 c + K p 9 e = 0. (11.10) 

Stability Error dynamics, such as Equation (11.10), are an important concept 
in the study of control systems. A desirable property is that the error dynamics 
be stable, i.e., initial errors tend to zero with time. A linear homogeneous 
ordinary differential equation of the form 

a n9g U ^ + a n -\9 ^ + ... + ai9 e + a\9 e + a>oO e = 0 

is stable if and only if all of the complex roots si,..., s n of its characteristic 
equation 

o n s n + a n _is n 1 + ... + CL 2 S “ + ais + do = 0 

have real components less than zero, i.e., Re(sj) < 0 for all * = 1... n. A 
necessary condition for stability, regardless of the order n of the dynamics, is 
that cii > 0 for all i. This condition is also sufficient for second-order dynamics 
such as Equation (11.10). For third-order dynamics, stability is assured if Oj > 0 
for all i and 0201 > 0 , 30 , 0 . 

PD Control and Second-Order Error Dynamics To study the second- 
order error dynamics (11.10) more formally, we assume stability and rewrite in 
the standard second-order form 

9 e -\ - — — -9 e + -^j9 e =0 —> 9 e + 2^uj n 9 e + = 0, (11.11) 

where the damping ratio ( and the natural frequency uj n are 

,_b + K d [Kp 

4 2^/KpM' ^" V M ' 

The characteristic equation of Equation (11.11) is 

s 2 + 2toj n s + u 2 n = 0, (11.12) 

with complex roots 

Sl,2 = ± UnVC 2 ~ !• 

There are three types of solutions to the differential equation (11.11), de¬ 
pending on whether the roots Sy 2 are real and unequal (£ > 1), real and equal 
(£ = 1), or complex conjugates (£ < 1): 

• Overdamped: £ > 1. The roots Sq 2 are real and distinct, and the 
solution is 


9 e (t) = cie Slt + c 2 e S2 * 
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where c i and ci depend on the initial conditions. The response is the sum 
of two decaying exponentials, with time constants r-1.2 = — 1/sj. 2, where 
the time constant is the time it takes the exponential to decay to 37% of 
its original value. The “slower” time constant in the solution is given by 
the less negative root, Si = — £w ra + ui n yj £ 2 — 1. 

• Critically damped: £ = 1. The roots S] 2 = —£are equal and real, 
and the solution is 

0 e {t) = (ci + c 2 t)e~ (u>nt , 

i.e., a decaying exponential multiplied by a linear function of time. The 
time constant of the decaying exponential is r = l/(£w n ). 

• Underdamped: £ < 1. The roots S 1.2 are complex conjugates at S\, 2 = 

— (w n ± jwd, where = u> n \J\ - Q' 2 is the damped natural frequency. 

The solution is 


0 e (t) = (ci cos (u] d t) + c 2 sin {u> d t)) e CUrit , 

i.e., a decaying exponential (time constant r = l/(£w n )) multiplied by a 
sinusoid. 


To see how to apply these solutions, imagine that the link is originally at 
rest at 9 = 0. At time t = 0, the desired position is suddenly changed from 
9d = 0 to 0^ = 1- This is called a step input and the resulting motion of the 
system 0(f) is called the step response. Our interest is in the error response 
0 e (£). We can solve for e- 1.2 in the specific solution by solving 0 e (0) = 1 (the 
error immediately becomes 1) and 0 e (0) = 0 (both 0^(0) and 0(0) are zero). 

The error response can be described by a transient response and a steady- 
state response (Figure 11.4). The steady-state response is characterized by 
the steady-state error e ss , which is the asymptotic error 0 e (£) as t — > 00 . For 
the link in zero gravity with a stable PD controller, e ss = 0. The transient 
response is characterized by the overshoot and (2%) settling time. The 2% 
settling time is the first time T such that |0 e (t) — e ss | < 0.02(1 — e ss ) for all 
t > T, and is approximately equal to 4r, where r is the slowest time constant 
in the solution. Overshoot occurs if the error response initially overshoots the 
final steady-state error, and in this case overshoot is defined as 


overshoot = 


e,min C-ss 
1 ^ss 


x 100% 


where 0 e . m i n is the least positive value achieved by the error. The overshoot can 
be calculated to be 

overshoot = e" x 10 q%, 0 < £ < 1. 


A good transient response is characterized by a low settling time and little or 
no overshoot. 
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Figure 11.4: The error response to a step input for an underdamped second- 
order system, showing steady-state error e ss , overshoot, and 2% settling time. 


Figure 11.5 shows the relationship of the location of the roots of Equa¬ 
tion (11.12) to the transient response. For a fixed I\d and a small K p , we have 
C > 1, the system is overdamped, and the response is sluggish due to the “slow” 
root. As K p is increased, the damping ratio decreases. The system is critically 
damped (( = 1) at K p = (b + K d ) 2 /(4M), and the two roots are coincident on 
the negative real axis. This situation corresponds to a relatively fast response 
and no overshoot. As K p continues to increase, £ drops below 1, the roots move 
off the negative real axis, and we begin to see overshoot and oscillation in the 
response. The settling time is unaffected as K p is increased beyond critical 
damping, as £ uj n is unchanged. In general, critical damping is desirable. 

Practical Bounds on Feedback Gains According to our simple model, 
we could increase K p and Kd without bound to make the real components 
of the roots more and more negative, achieving arbitrarily fast response. In 
practice, however, large gains lead to actuator saturation, rapid torque changes 
(chattering), vibrations of the structure due to unmodeled flexibility in the joints 
and links, and possibly even instability due to the finite servo rate frequency. 
Thus there are practical limits on the set of useful gains. 

PID Control and Third-Order Error Dynamics Now consider the case 
of setpoint control where the link moves in a vertical plane, i.e., g > 0. With 
the PD control law above, the system can now be written 

M0 e + (b + Kd)0 e + K p 9 e = mgr cos 9. 


(11.13) 
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Figure 11.5: (Left) The complex roots of the characteristic equation of the PD- 
controlled joint for a fixed K d = 10 Nms/rad as K p increases from zero. This is 
known as a “root locus” plot. (Right) The response of the system to an initial 
error 9 e = 1, 9 e = 0 is shown for overdamped (£ = 1.5, roots at “1”), critically 
damped (( = 1, roots at “2”), and underdamped (( = 0.5, roots at “3”) cases. 


This implies that the system comes to rest at a configuration 9 satisfying K p 9 e = 
mgr cos 9, i.e., the final error 9 e is not zero when 9d ^ ±f. This is because the 
robot must provide a nonzero torque to hold the link at rest at 9 ^ ± ^, but the 
PD control law only creates a nonzero torque at rest if 9 e 0. We can make 
this steady-state error small by increasing the gain K p , but as discussed above, 
there are practical limits. 

To eliminate the steady-state error, we return to the PID controller by setting 
Ki > 0. This allows a nonzero steady-state torque even with zero position 
error; only the integrated error must be nonzero. Figure 11.6 demonstrates the 
addition of the integral term to the controller. 

To see how this works, write the setpoint error dynamics 


M9 e + {b + K d )9 e + K p 9 e + Ki J 9 e (t)dt = r dist , (11.14) 

where T d j st is a disturbance torque substituted for the gravity term mgr cos 0. 
Taking derivatives of both sides, we get the third-order error dynamics 

M9 '< 3 > +(b + K d )9 e + K p 9 e + K t 9 e = r dist . (11.15) 

If Tdist is constant, then the right-hand side of Equation (11.15) is zero. If the 
PID controller is stable, then (11.15) shows that 9 e converges to zero. (While 
the disturbance torque due to gravity is not constant as the link rotates, it 
approaches a constant as 9 approaches zero, and therefore similar reasoning 
holds close to the equilibrium.) 

Integral control is useful to eliminate steady-state error in setpoint control, 
but it may adversely affect the transient response. This is because integral 
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fijl 


desired configuration = 
PID final configuration 



PD final configuration 


initial configuration 


Figure 11.6: (Left) The tracking errors for a PD controller with K d = 
2 Nms/rad, K p = 2.205 Nm/rad for critical damping, and a PID controller 
with the same PD gains and Ki = 1 Nm/(rad s). The arm starts at 9(0) = 
—7t/2,0(O) = 0, with a goal state dd = 0 ,9 d = 0. (Middle) The individual 
contributions of the terms in the PD and PID control laws. (Right) The initial 
and final configurations. 


control essentially responds to delayed information—it takes time for the system 
to respond to error as it integrates. It is well known in control theory that 
delayed feedback can cause instability. To see this, consider the characteristic 
equation of Equation (11.15) when T^ist is constant: 

Ms 3 + (b + K d )s 2 + K p s + Ki = 0. (11.16) 

For all roots to have negative real part, we require b + K d > 0 and K p > 0 as 
before, but there is also an upper bound on the new gain 7Q (Figure 11.7): 

“ 1 M 

Thus a reasonable design strategy is to choose K p and K d for a good transient 
response, then choose Ki small so as not to adversely affect stability. In the 
example of Figure 11.6, the relatively large Ki worsens the transient response, 
giving significant overshoot. In practice, 7Q = 0 for many robot controllers. 
Pseudocode for the PID control algorithm is given in Figure 11.8. 

While our analysis has focused on setpoint control, the PID controller applies 
perfectly well to trajectory following, where 9 d (t) ^ 0. Integral control will not 
eliminate tracking error along arbitrary trajectories, however. 

11.2.2.2 Feedforward Control 

Another strategy for trajectory following is to use a model of the robot’s dy¬ 
namics to proactively generate torques, instead of waiting for errors. Let the 
controller’s model of the dynamics be 

t = M(9)9 + h(9,9), 


(11.17) 
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Figure 11.7: The three roots of Equation (11.16) as I\i increases from zero. First 
a PD controller is chosen with K p and K d yielding critical damping, giving rise 
to two collocated roots on the negative real axis. Adding an infinitesimal gain 
Ki > 0 creates a third root at the origin. As we increase Ki, one of the two 
collocated roots moves to the left on the negative real axis, while the other two 
roots move toward each other, meet, break away from the real axis, begin curving 
to the right, and finally move into the right-half plane when Ki = (b+K d )K p /M. 
The system is unstable for larger values of Ki. 


where the model is perfect if M(9) = M(9) and h(9,9) = h(9,9). Note that 
the inertia model M(9) is written as a function of the configuration 9. While 
the inertia of our simple one-joint robot is not a function of configuration, writ¬ 
ing this way allows us to re-use Equation (11.17) for multi-joint systems in 
Section 11.2.3. 

Given 9 d , 9d , and 9d from the trajectory generator, the commanded torque 
is calculated as 


T = M(9 d )9 d + h(9 d ,9 d ). 


(11.18) 


If the model of the robot dynamics is exact, and there are no initial state errors, 
then the robot exactly follows the desired trajectory. This is called feedforward 
control, as no feedback is used. 

A pseudocode implementation of feedforward control is given in Figure 11.9. 

Figure 11.10 shows two examples of trajectory following for the link in grav¬ 
ity. Here, the controller’s dynamic model is correct except that it has r = 0.08 m, 
when actually r = 0.1 m. In Task 1, the error stays small, as unmodeled gravity 
effects provide a spring-like force to 9 = — 7t/2, accelerating the robot at the be¬ 
ginning and decelerating it at the end. In Task 2, unmodeled gravity effects act 
against the desired motion, resulting in a larger tracking error. Because there 
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time = 0 

// 

dt = servo cycle time 

eint = 0 

// 

error integral 

qprev = senseAngle 

// 

initial joint angle q 

loop 




[qd,qdotd] = trajectory(time) // from trajectory generator 

q = senseAngle // sense actual joint angle 

qdot = (q - qprev)/dt // simple velocity calculation 

qprev = q 

e = qd - q 

edot = qdotd - qdot 

eint = eint + e*dt 

tau = Kp*e + Kd*edot + Ki*eint 
commandTorque(tau) 

time = time + dt 
end loop 


Figure 11.8: Pseudocode for PID control. 


time =0 // dt = servo cycle time 

loop 

[qd,qdotd,qdotdotd] = trajectory(time) // trajectory generator 

tau = Mtilde(qd)*qdotdotd + htilde(qd,qdotd) // calculate dynamics 
commandTorque(tau) 
time = time + dt 
end loop 


Figure 11.9: Pseudocode for feedforward control. 


are always modeling errors, feedforward control is always used in conjunction 
with feedback, as discussed next. 

11.2.2.3 Feedforward Plus Feedback Linearization 

All practical controllers use feedback, as no model of robot and environment 
dynamics will be perfect. Nonetheless, a good model can be used to improve 
performance and simplify analysis. 

Let’s combine PID control with a model of the robot dynamics {M, h} to 
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Task 1 






Figure 11.10: Results of feedforward control with an incorrect model: f = 
0.08 m, but r = 0.1 m. The desired trajectory in Task 1 is 9 d (t) = — 7r/2 — 
(7r/4)cos(t) for 0 < t < n. The desired trajectory for Task 2 is 9d(t) = n/2 — 
(7t/4) cos(f), 0 <t< 7T. 


achieve the error dynamics 


9 e + K d 9 e + K p 6 e + Kij 9 e (t)dt = 0 (11.19) 

along arbitrary trajectories, not just to a setpoint. The error dynamics (11.19) 
and proper choice of PID gains ensure exponential decay of trajectory error. 

Since 9 e = 9 d — 9 , to achieve the error dynamics (11.19), we choose the 
robot’s commanded acceleration to be 

9com = 9 d — 9 e then plug in Equation (11.19) to get 

= 9 d + K d 9 e + K p 9 e + Ki f 9 e (t)dt. (11.20) 


Plugging 9 CO m into a model of the robot dynamics {M, h}, we get the feed¬ 
forward plus feedback linearizing controller, also called the computed 
torque controller: 


r = M(9) 


^' 9 d + K P 9 e + Ki 


9 e {t)dt + K d 9 


+ h(9,6). 


( 11 . 21 ) 
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Figure 11.11: Computed torque control. The feedforward acceleration 9 d is 
added to the acceleration 0fb computed by the PID feedback controller to create 
the commanded acceleration 9 com . 





Figure 11.12: Performance of feedforward only (ff), feedback only (fb), and 
computed torque control (ff+fb). PID gains are taken from Figure 11.6, and 
the feedforward modeling error is taken from Figure 11.10. The desired motion 
is Task 2 from Figure 11.10. The middle plot shows the tracking performance 
of the three controllers. Also plotted is f r 2 (f)df, a standard measure of control 
effort, for each of the three controllers. These plots show typical behavior: the 
computed torque controller yields better tracking than either feedforward or 
feedback alone, with less control effort than feedback alone. 


This controller includes a feedforward component, due to the use of the planned 
acceleration 9 d , and is called feedback linearizing because feedback of 9 and 9 
is used to generate linear error dynamics. The h{9, 9) term cancels dynamics 
that depend nonlinearly on the state, and the inertia model M(9) converts 
desired joint accelerations into joint torques, realizing the simple linear error 
dynamics (11.19). 

A block diagram of the computed torque controller is shown in Figure 11.11. 
The gains K pi Ki, and K d are chosen to place the roots of the characteristic 
equation as desired to achieve good transient response. Under the assumption 
of a perfect dynamic model, we would choose Ki = 0. 

Figure 11.12 shows typical behavior of feedback linearizing control relative 
to feedforward and feedback only. Pseudocode is given in Figure 11.13. 
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time =0 // dt = cycle time 

eint =0 // error integral 

qprev = senseAngle // initial joint angle q 

loop 

[qd,qdotd,qdotdotd] = trajectory(time) // from trajectory generator 

q = senseAngle // sense actual joint angle 

qdot = (q - qprev)/dt // simple velocity calculation 

qprev = q 

e = qd - q 

edot = qdotd - qdot 

eint = eint + e*dt 

tau = Mtilde(q)*(qdotdotd + Kp*e + Kd*edot + Ki*eint) + htilde(q.qdot) 
commandTorque(tau) 

time = time + dt 
end loop 


Figure 11.13: Pseudocode for the computed torque controller. 


11.2.3 Motion Control of a Multi-joint Robot with Torque or 
Force Input 

The methods applied above for a single-joint robot carry over directly to n-joint 
robots. The difference is that the dynamics (11.6) now take the more general 
vector-valued form 

r = M{6)6 + h(6,6), 

where the nxn positive-definite mass matrix M is now a function of the configu¬ 
ration 6. We will sometimes find it convenient to explicitly write the components 
of the term h(6,6), 

t = M{6)6 + C(0, 0)0 + g{0) + b(0), (11.22) 

where C(8, 6)6 are Coriolis and centripetal terms, g{6) are potential (e.g., grav¬ 
ity) terms, and b(6) are friction terms. In general, the dynamics (11.22) are 
coupled—the acceleration of a joint is a function of the positions, velocities, 
and torques at other joints. 

We distinguish between two types of control of multi-joint robots: decen¬ 
tralized control, where each joint is controlled separately with no sharing of 
information between joints, and centralized control, where full state informa¬ 
tion for each of the n joints is available to calculate the controls for each joint. 
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11.2.3.1 Decentralized Multi-Joint Control 

The simplest method for controlling a multi-joint robot is to apply an inde¬ 
pendent controller at each joint, such as the single-joint controllers discussed 
in Section 11.2.2. Decentralized control is appropriate when the dynamics are 
decoupled, or at least approximately decoupled. The dynamics are decoupled 
when the acceleration of each joint depends only on the torque, position, and 
velocity of that joint. This requires that the mass matrix be diagonal, as in 
Cartesian or gantry robots, where the first three axes are prismatic and orthog¬ 
onal along the x-y-z axes. This kind of robot is equivalent to three single-joint 
systems. 

Approximate decoupling is also achieved in highly geared robots in the ab¬ 
sence of gravity. The mass matrix M{9) is nearly diagonal, as it is dominated 
by the apparent inertias of the motors themselves (see Chapter 8.9.2). Signif¬ 
icant friction at the individual joints also contributes to the decoupling of the 
dynamics. 

11.2.3.2 Centralized Multi-Joint Control 

When gravity forces and torques are significant and coupled, or when the mass 
matrix M(9) is not well approximated by a diagonal matrix, decentralized con¬ 
trol may not yield acceptable performance. In this case, the computed torque 
control law (11.21) of Figure 11.11 can be generalized. The configurations 9 and 
Od and the error 9 e = 9d — 9 are now n-vectors, and the positive scalar gains 
become positive-definite matrices K p , Ki, Kd'. 



(11.23) 


Typically we choose the gain matrices as k p I , fcj/, where I is the n x n 
identity matrix and k p ,ki, and kd are nonnegative scalars. Commonly Ki is 
chosen to be zero. In the case of an exact dynamics model M and h, the dy¬ 
namics of each joint reduces to the linear dynamics (11.19). The block diagram 
and pseudocode for this control algorithm are found in Figures 11.11 and 11.13, 
respectively. 

Implementing the control law (11.23) requires calculating potentially com¬ 
plex dynamics. We may not have a good model of these dynamics, or the 
equations may be too computationally expensive to calculate at servo rate. In 
this case, if the desired velocities and accelerations are small, an approximation 
to (11.23) can be obtained using only PID control and gravity compensation: 



(11.24) 
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With zero friction, perfect gravity compensation, and PD setpoint control (Ki = 
0 and 9 d = 9 d = 0), the controlled dynamics can be written 

M(9)9 + C(9,9)9 = K p 9 e - K d 9 , (11.25) 

where the Coriolis and centripetal terms are written C(9,9)0. We can now 
define a virtual “error energy,” which is the sum of an “error potential energy” 
stored in the virtual spring K p and an “error kinetic energy”: 

V{9 ei 9 e ) = l -0 T e K p 9 e + l -9 T e M(9)9 e . (11.26) 

Since 9 d = 0, this reduces to 

V{9 e , 9) = \ylK p 9 e + l -9 T M{9)9. (11.27) 

Taking the time derivative and plugging in (11.25), we get 

V = - 9 T K p 9 e + 9 t M(9)9 + ^9 T M(9)9 

= —9 T K p 9 e + 9 t ( K p 9 e - K d 9 - C{9,9)9 ) + ^ 9 T M(9)9. (11.28) 

Rearranging, and using the fact that M — 2 C is skew-symmetric (Proposi¬ 
tion 8.1.2), we get 

V = - 9 T K p 9 e + 9 t ( K p 9 e - K d 9 ) + \o t {m, 

= ~9 T K d 9 < 0. 

This shows that the error energy is decreasing when S / 0. If 9 — 0 and 
9 ^ 9 d , the virtual spring ensures that 9 ^ 0, so 9 e will again become nonzero 
and more energy will be dissipated. Thus by the Krasovskii-LaSalle invariance 
principle (Exercise 12), the total error energy decreases monotonically and the 
robot converges to rest at 9 d ( 9 e = 0) from any initial state. 

11.2.4 Task Space Motion Control 

In Section 11.2.3, we focused on motion control in joint space. This is convenient 
because joint limits are easily expressed in this space, and the robot should be 
able to execute any joint-space path respecting these limits. Trajectories are 
naturally described by the joint variables, and there are no issues of singularities 
or redundancy. 

On the other hand, since the robot interacts with the external environment 
and objects in it, it may be more convenient to express the motion as a trajectory 
of the end-effector in task space. Let the end-effector trajectory be specified by 


(9,9)) 9 

(11.29) 
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(X(t),V(t)), where X £ SE( 3) or X £ R”, for example, with V £ R n the 
velocity. Provided the corresponding trajectory in joint space is feasible, we 
now have two options for control: ( 1 ) convert to a joint-space trajectory and 
proceed with control as in Section 11.2.3 or (2) express the robot dynamics and 
control law in the task space. 

The first option is to convert the trajectory to joint space. The forward 
kinematics are X = f(9 ) and V = J{9)9 , where J(9) is the appropriate Jacobian 
based on the chosen velocity representation V. Then the joint space trajectory 
is obtained from the task space trajectory using inverse kinematics (Chapter 6 ): 

(inverse kinematics) 9(t) = f~ 1 (X(t)) (11.30) 

0(t) = ji(0(t))V(t) (11.31) 

9(t) = Jt( 0 ( t )) (l>(f) - j(9(t))9(t)) . (11.32) 

A drawback of this approach is that we must calculate the inverse kinematics, 
which may require significant computing power. The second option is to express 
the robot’s dynamics in task-space coordinates, as discussed in Chapter 8 . 6 . 
Recall the task-space dynamics 

T = A{9)V + r](9,V). 

The joint forces and torques r are related to the forces T expressed in the 
end-effector frame by r = J T (9)J-. 

We can now write a control law in task coordinates inspired by the computed 
torque control law in joint coordinates (11.23), 


r = J t {9) ^A(0) + K p X e + KiJ X e (t)dt + K d V e ^j + fj(9, V) j , 

(11.33) 

where V d is the desired acceleration and {A, 77 } represents the controller’s dy¬ 
namics model. 

The task-space control law (11.33) makes use of the configuration error X e 
and velocity error V e . When X is expressed in a minimal set of coordinates 
(. X £ R m ) and V = X, a natural choice is X e = X d — X, V e = X d — X. When 
X = ( R,p ) £ SE(3), however, there are a number of possible choices, including 
the following: 

• V = Vh and J(9) = Jb{9): twists are represented in the end-effector frame 
{b}. A natural choice would use X e such that [X e ] = log(Jf _ 1 Xd) and 
V e = [Adx-i^jVd — V. The twist X e £ R 6 is the velocity, expressed in 
the end-effector frame, which, if followed for unit time, would move the 
current configuration X to the desired configuration X d . The transform 
[Adx-ixJ expresses the desired twist V d , which is expressed in the frame 
X d , as a twist in the end-effector frame at X, in which the actual velocity 
V is represented, so the two can be differenced. 
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• V and J(Q) chosen so that V = (w,u), where u ; is the angular velocity 
of the end-effector expressed in the {s} frame and v = p. The matrix 
representation of the angular velocity uib that takes R to Rd in unit time, 
expressed in the end-effector frame {b}, is [w*,] = log (R T Rd), so a natural 
choice for error coordinates would be 


X e 


Rujb 

Pd-P 


and V e = V d - V, 


where Ru>b is the angular velocity that takes R to Rd in unit time, ex¬ 
pressed in {s}. 

These choices lead to different behaviors of the robot. In particular, the second 
choice decouples the rotational and linear corrective terms. 


11.3 Force Control 

When the goal is not to create motions at the end-effector, but to apply forces 
and torques to the environment, the task requires force control. Pure force 
control is only possible if the environment provides resistance forces in every 
direction (e.g., when the end-effector is embedded in concrete or attached to 
a spring providing resistance in every motion direction). Pure force control is 
a bit of an abstraction, as robots are usually able to move freely in at least 
some direction. It is a useful abstraction, however. It also leads to hybrid 
motion-force control in Section 11.4. 

In ideal force control, the force applied by the end-effector is unaffected by 
disturbance motions applied to the end-effector. This is dual to the case of ideal 
motion control, where the motion is unaffected by disturbance forces. Force 
control is dual to motion control in the sense that forces are dual to velocities, 
with their product being power, an intrinsic, coordinate-free concept. 

Let Jq ip be the wrench that the manipulator applies to the environment. 
The manipulator dynamics can be written 

M(9)9 + c(6,0) + g(0) + b{0) + J T {0)R tip = r, (11.34) 

where the Jacobian J(6) satisfies V = J{9)9. Since the robot typically moves 
slowly (or not at all) during a force control task, we ignore the acceleration and 
velocity terms to get 

g(0) + J T (0) T tip = t. (11.35) 

In the absence of any direct measurements of the force-torque at the robot 
end-effector, joint angle feedback alone can be used to implement the force 
control law 

r = m + J T (0)T d , (H.36) 

where g{6) is the model of gravitational torques and J~d is the desired wrench. 
This control law requires a good model for gravity compensation as well as 
precise control of the torques produced at the robot joints. In the case of a DC 
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Figure 11.14: A six-axis force-torque sensor, highlighted in yellow, mounted 
between the Barrett WAM robot arm and its end-effector. 

electric motor without gearing, torque control can be achieved by current control 
of the motor. In the case of a highly geared actuator, however, large friction 
torque in the gearing can degrade the quality of torque control achieved using 
only current control. In this case, the output of the gearing can be instrumented 
with strain gauges to directly measure the joint torque, which is fed back to a 
local controller that modulates the motor current to achieve the desired output 
torque. 

Another solution is to equip the robot arm with a six-axis force-torque sen¬ 
sor between the arm and the end-effector to directly measure the end-effector 
wrench .7-tip (Figure 11.14). Force-torque measurements are often noisy, so the 
time derivative of these measurements may not be meaningful. In addition, the 
desired wrench T,i is typically constant or only slowly changing. These char¬ 
acteristics suggest a PI force controller with a feedforward term and gravity 
compensation, 



(11.37) 


where T e = Fd — F t \ v and Kf p and Kfi are positive-definite proportional and 
integral gain matrices, respectively. In the case of perfect gravity modeling, 
plugging the force controller (11.37) into the dynamics (11.35), we get the error 
dynamics 



(11.38) 


In the case of a constant force disturbance on the right-hand side of (11.38), 
arising from an incorrect model of g(9), for example, we take the derivative to 
get 


Kf p F g A FfiF g — 0, 


(11.39) 
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showing that J~ e converges to zero for positive-definite Kf p and Kfi. 

The control law (11.37) is simple and appealing, but potentially dangerous if 
incorrectly applied. If there is nothing for the robot to push against, the robot 
will accelerate in a failing attempt to create end-effector forces. Since a typical 
force control task requires little motion, we can limit this acceleration by adding 
velocity damping. This gives the modified control law 


(11.40) 


where -Kd amp is positive definite. 

11.4 Hybrid Motion-Force Control 

Most tasks requiring the application of controlled forces also require the applica¬ 
tion of controlled motions. Control to achieve this is called hybrid motion-force 
control. If the the task space is n-dimensional, then we are free to specify n 
of the 2 n forces and motions at any time i; the other n are determined by the 
environment. Apart from this constraint, we also should not specify forces and 
motions in the “same direction,” as they are not independent. 

As an example, consider a two-dimensional environment modeled by a damper, 
T = B e nv V, where 

'2 1 ~ 

1 1 


Benv — 


Defining the components of V and T as (Vi, V 2 ) and 2 ), we have T\ = 

2Vi + V 2 , J -2 = Vi +V 2 - We have n = 2 freedoms to choose among the 2n = 4 
velocities and forces at any time. For example, we can specify both T\ and Vi 
independently, because B env is not diagonal. Then V 2 and Ti are determined 
by -Benv We cannot independently control both T\ and 2Vi + V 2 , however, as 
these are in the “same direction” according to the damper. 


11.4.1 Natural and Artificial Constraints 

A particularly interesting case is when the environment is infinitely stiff (rigid 
constraints) in k directions and unconstrained inn - fc directions. In this case, 
we cannot choose which of the 2 n motions and forces to specify—the contact 
with the environment chooses the k directions in which the robot can freely 
apply forces and the n — k directions of free motion. As an example, consider 
the task space to have the n = 6 dimensions of SE(3). Then a robot opening a 
cabinet door has 6 — k = 1 motion freedom, rotation about the cabinet hinges, 
and therefore k = 5 force freedoms—the robot can apply any wrench that has 
zero moment about the axis of the hinges without moving the door. 

As another example, a robot writing on a chalkboard may freely control the 
force into the board (k = 1), but it cannot penetrate the board; and it may 
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freely move with 6 — k = 5 degrees of freedom (two specifying the motion of the 
tip of the chalk in the plane of the board, and three describing the orientation 
of the chalk), but it cannot independently control forces in these directions. 

The chalk example comes with two caveats. The first is due to friction- 
the chalk-wielding robot can actually control forces tangent to the plane of the 
board, provided the requested motion in the plane of the board is zero and the 
requested tangential forces do not exceed the static friction limit determined by 
the friction coefficient and the normal force into the board (see friction model¬ 
ing in Chapter 12). Within this regime, the robot has three motion freedoms 
and three force freedoms. Second, the robot could decide to pull away from the 
board. In this regime, the robot has six motion freedoms and no force freedoms. 
Thus the configuration of the robot is not the only factor determining the di¬ 
rections of motion and force freedoms. Nonetheless, in this section we consider 
the simplified case where the motion and force freedoms are determined solely 
by the robot’s configuration, and all constraints are equality constraints. For 
example, the inequality velocity constraint of the board (the chalk cannot pen¬ 
etrate the board) is treated as an equality constraint (the robot also does not 
pull the chalk away from the board). 

As a final example, consider a robot erasing a frictionless chalkboard using 
an eraser modeled as a rigid block (Figure 11.15). Let the configuration X(t) £ 
SE( 3) be the configuration of the block’s frame {b} relative to a space frame 
{s}. The body-frame twist and wrench are written 14 = (u] x ,ui y ,u) z ,v x ,v y ,v z ) T 
and Fb = (m x ,m y ,m z ,f x ,f y ,f z ) T 1 respectively. Maintaining contact with the 
board puts k = 3 constraints on the twist: 

uj x = 0 

UJy = 0 

v z = 0. 

In the language of Chapter 2, these velocity constraints are holonomic—the 
differential constraints can be integrated to give configuration constraints. 

These constraints are called natural constraints, specified by the en¬ 
vironment. There are 6 — k = 3 natural constraints on the wrench, too: 
m z = f x = f y = 0. In light of the natural constraints, we can freely specify 
any twist of the eraser satisfying the k = 3 velocity constraints and any wrench 
satisfying the 6 — k = 3 wrench constraints (provided f z < 0, to maintain con¬ 
tact with the board). These motion and force specifications are called artificial 
constraints. Below is an example set of artificial constraints corresponding to 
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Figure 11.15: The fixed space frame {s} attached to the chalkboard and the 
body frame {b} attached to the center of the eraser. 


the natural constraints: 

natural constraints 
CJ X d 
Uly = 0 
m z = 0 

/* = o 

fy = 0 

v z =0 

The artificial constraints cause the en 
a constant force k 2 against the board. 


artificial constraints 
m x — 0 

Uly = 0 

ui z =0 
v x = ki 

v v = 0 

fz = k 2 < 0 

to move with v x = k\ while applying 


11.4.2 A Hybrid Controller 

We now return to the problem of designing a hybrid motion-force controller. If 
the environment is rigid, then we express the k natural constraints on velocity 
in task space as the Pfafhan constraints 

A(X)V = 0, (H-41) 

where A{X) £ anc j m = Q f or X £ SE( 3). (Alternatively these con¬ 

straints could be written in terms of constraints on the joint velocities, A(0)9 = 
0.) This formulation includes holonomic and nonholonomic constraints. 

If the task-space dynamics of the robot, in the absence of constraints, are 


T = A(0)V + t7(0,V), 
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then the constrained dynamics, following Chapter 8.7, are 

T = A(0)V + r ? (0,V) + A^(X)A, (11.42) 

F tip 

where AgR 4 are Lagrange multipliers and J-jip is the set of forces (or wrench) 
that the robot applies against the constraints. The requested force T,i must lie 
in the column space of A T (X). 

Since Equation (11.41) must be satisfied at all times, we can replace (11.41) 
by the time derivative 

A{X)V + A{X)V = 0. (11.43) 

To ensure that Equation (11.43) is satisfied when the system state already sat¬ 
isfies A{X)V = 0, any requested acceleration Vd should satisfy A{X)Vd = 0. 

Now, solving Equation (11.42) for V, plugging the result into (11.43), and 
solving for A, we get 

A = (. AA~ 1 A t )~ 1 (AV + AA-^rj - T)), (11.44) 

where we have plugged in — AV = AV by Equation (11.43). With Equa¬ 
tion (11.44), we can calculate the force = A T (q) A the robot applies against 
the constraints. 

Plugging Equation (11.44) into Equation (11.42) and manipulating, the n 
equations of the constrained dynamics (11.42) can be expressed as the n — k 
independent motion equations 

P{X)F = P(X)( A(0)V + 77 ( 0 , V)) (11.45) 

where 

P = I - A t (AA- 1 A t )- 1 AA~ 1 (11.46) 

and / is the identity matrix. The nxn matrix P(X) has rank n — k and projects 
an arbitrary manipulator force T onto the subspace of forces that move the end- 
effector tangent to the constraints. The rank k matrix I — P(X) projects an 
arbitrary force J- to the subspace of forces against the constraints. Thus P 
partitions the n-dimensional force space into forces that address the motion 
control task and forces that address the force control task. 

Our hybrid motion-force controller is simply the sum of a task-space motion 
controller, derived from the computed torque control law (11.33), and a task- 
space force controller (11.37), each projected to generate forces in its appropriate 
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subspace: 


t = J t (6) P(X) A(0) 


V d + K p X e + Ki / X e (t)dt + K d V e 


motion control 


+ (J - P(X)) (r d + K fp P e + K fi J P e {t)d?j 


force control 

nonlinear compensation 


(11.47) 


Because the dynamics of the two controllers are decoupled by the orthogonal 
projections P and I — P, the controller inherits the error dynamics and stabil¬ 
ity analyses of the individual force and motion controllers on their respective 
subspaces. 

A difficulty in implementing the hybrid control law (11.47) in rigid envi¬ 
ronments is knowing precisely the constraints A(X)V = 0 active at any time. 
This is necessary to specify the desired motion and force and to calculate the 
projections, but any model of the environment will have some uncertainty. One 
approach to dealing with this issue is to use a real-time estimation algorithm to 
identify the constraint directions based on force feedback. Another is to sacri¬ 
fice some performance by choosing low feedback gains, which makes the motion 
controller “soft” and the force controller more tolerant of force error. We can 
also build passive compliance into the structure of the robot itself to achieve 
a similar effect. In any case, some passive compliance is unavoidable, due to 
flexibility in the joints and links. 


11.5 Impedance Control 

Ideal hybrid motion-force control in rigid environments demands extremes in 
robot impedance, which characterizes the change in endpoint motion as a func¬ 
tion of disturbance forces. Ideal motion control corresponds to high impedance 
(little change in motion due to force disturbances) while ideal force control cor¬ 
responds to low impedance (little change in force due to motion disturbances). 
In practice, there are limits to a robot’s achievable impedance range. 

In this section, we consider the problem of impedance control, where the 
robot end-effector renders particular mass, spring, and damper properties. 3 For 
example, a robot used as a haptic surgical simulator could be tasked with mim¬ 
icking the mass, stiffness, and damping properties of a virtual surgical instru¬ 
ment in contact with virtual tissue. 

A one-degree-of-freedom robot rendering an impedance can be written 

mi + bx + kx = /, (11.48) 

3 A popular subcategory of impedance control is stiffness control or compliance control, 
where the robot renders a virtual spring only. 
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Figure 11.16: A robot creating a one-degree-of-freedom mass-spring-damper 
virtual environment. The human hand applies a force / to the haptic interface. 


where x is the position, m is the mass, b is the damping, k is the stiffness, 
and / is the force applied by the user (Figure 11.16). Loosely, we say that the 
robot renders high impedance if one or more of the {m, 6, k} parameters, usually 
including b or fc, is large. Similarly, we say that the impedance is low if all of 
these parameters are small. 

More formally, taking the Laplace transform of Equation (11.48), we get 

(m s 2 + bs + k)X(s) = F(s), (11.49) 

and the impedance is defined by the transfer function from position pertur¬ 
bations to forces, Z(s) = F(s)/X(s). Thus impedance is frequency depen¬ 
dent, with low-frequency response dominated by the spring and high-frequency 
response dominated by the mass. Admittance is the inverse of impedance, 
Y(s) = Z-\s)=X(s)/F(s). 

A good motion controller is characterized by high impedance (low admit¬ 
tance), since AX = Y A F. If the admittance Y is small, then force perturbations 
A F produce only small position perturbations AX. Similarly, a good force con¬ 
troller is characterized by low impedance (high admittance), since A F = ZAX, 
and small Z implies that motion perturbations produce only small force pertur¬ 
bations. 

The goal of impedance control is to implement the task-space behavior 

MV + BV + KX = F ext , (11.50) 

where X € M” and V = X are the task-space position and velocity 4 ; M, B , and 
K are the positive-definite virtual mass, damping, and stiffness to be created 
by the robot; and F ex t is a force applied to the robot, perhaps by a user. The 
values of M, B , and K may change depending on the location in the virtual 
environment, to represent distinct objects for instance, but we will focus on 
the case of constant values. We could also replace V, V, and X with small 
displacements AV, AV, and AX from reference values in a controlled motion 
of the robot, but we dispense with the extra notation here. 

There are two common ways to achieve the behavior (11.50): 

4 In this section we consider a simple coordinate description of the configuration of the 
virtual mass, such as X £ R 3 . The stiffness matrix K for a rigid body whose position is 
represented as X £ SE( 3) is addressed in [42, 68]. 
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• The robot senses the motion X(t) and commands joint torques and forces 
to create —lF ext , the force to display to the user. Such a robot is called 
impedance controlled, as it implements a transfer function Z(s ) from 
motions to forces. Theoretically, an impedance-controlled robot should 
only be coupled to an admittance-type environment. 

• The robot senses T ext using a wrist force-torque sensor and controls mo¬ 
tions in response. Such a robot is called admittance controlled, as it 
implements a transfer function Y ( s ) from forces to motions. Theoretically, 
an admittance-controlled robot should only be coupled to an impedance- 
type environment. 

11.5.1 Impedance Control Algorithm 

In an impedance control algorithm, encoders, tachometers, and possibly ac¬ 
celerometers are used to estimate the joint and endpoint positions, veloci¬ 
ties, and possibly accelerations. Often impedance-controlled robots are not 
equipped with a wrist force-torque sensor, and instead rely on their ability to 
precisely control joint torques to display the appropriate end-effector force —T ex t 
(from (11.50)) to the user. A good control law might be 



( \ 

r = J T (0) 

A(0)V + rj(0, V) - (MV + BV + KX) . 


^ arm dynamics compensation J~ex.t J 


(11.51) 


Addition of an end-effector force-torque sensor allows the use of feedback terms 
to more closely achieve the desired interaction force — T ex t. 

In the control law (11.51), it is assumed that V, V, and X are measured 
directly. Measurement of the acceleration V is likely to be noisy, however, and 
there is the problem of attempting to compensate for the robot’s mass after 
the acceleration has been sensed. Therefore, it is not uncommon to eliminate 
the mass compensation term A(0)V and to set M = 0. The mass of the arm 
will be apparent to the user, but impedance-controlled manipulators are often 
designed to be lightweight. It is also not uncommon to assume small velocities 
and replace the nonlinear dynamics compensation f/(0, V) with a simpler gravity 
compensation model. 

Problems can arise when (11.51) is used to simulate stiff environments. Small 
changes in position, as measured by encoders for example, lead to large changes 
in motor torques. This effective high gain, coupled with delays, sensor quan¬ 
tization, and sensor errors, can lead to oscillatory behavior or instability. On 
the other hand, the effective gains are low when emulating low impedance envi¬ 
ronments. A lightweight backdrivable manipulator can excel at emulating low 
impedance environments. 
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11.5.2 Admittance Control Algorithm 

In an admittance control algorithm, the force lF ey ± applied by the user is sensed 
by the wrist load cell, and the robot responds with an end-effector acceleration 
satisfying Equation (11.50). A simple approach is to calculate the desired end- 
effector acceleration Vd according to 

MV d + BV + KX 

— BXt 1 

where (X, V) is the current state. Solving, we get 

V d = M~ 1 {T ext -BV-KX). (11.52) 

Given Vd, V, and 6, the dynamics in the task space (8.88) can be used to 
determine the commanded wrench J-. To make the response smoother in the 
face of noisy force measurements, the force readings can be low-pass filtered. 

Simulating a low impedance environment is challenging for an admittance- 
controlled robot, as small forces produce large accelerations. The effective large 
gains can produce instability. On the other hand, admittance control by a highly 
geared robot can excel at emulating stiff environments. 


11.6 Other Topics 

Robust Control While all stable feedback controllers confer some amount of 
robustness to uncertainty, the field of robust control deals with designing con¬ 
trollers that explicitly guarantee the performance of a robot subject to bounded 
parametric uncertainties, such as in its inertial properties. 

Adaptive Control Adaptive control of robots involves estimating the 
robot’s inertial or other parameters during execution and updating the control 
law in real time to incorporate those estimates. 

Iterative Learning Control Iterative learning control (ILC) generally 
focuses on repetitive tasks. If a robot performs the same pick-and-place oper¬ 
ation over and over, the trajectory errors from the previous execution can be 
used to modify the feedforward control for the next execution. In this way, the 
robot improves its performance over time, driving execution error toward zero. 
ILC differs from adaptive control in that the “learned” information is generally 
nonparametric and useful only for a single trajectory. On the other hand, ILC 
can account for effects that have not been parametrized in a model. 

Passive Compliance and Flexible Manipulators All robots unavoidably 
have some passive compliance. Models of this compliance can be as simple as 
assuming torsional springs at each revolute joint (e.g., to account for finite stiff¬ 
ness in the flexsplines of harmonic drive gearing) or as complicated as treating 
links as flexible beams. Two significant effects of flexibility are (1) a mismatch 
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between the motor angle reading, the true joint angle, and the endpoint of the 
attached link, and (2) increased order of the dynamics of the robot. These issues 
raise challenging problems in control, particularly when vibration modes are at 
low frequencies. 

Some robots are specifically designed for passive compliance, particularly 
those meant for contact interactions with humans or the environment. Such 
robots may sacrifice motion control performance in favor of safety. One passively 
compliant actuator is the series elastic actuator, which consists of an electric 
motor, a gearbox, and a torsional or linear spring connecting the gearbox output 
to the link. This spring provides passive compliance between the stiff, highly 
geared motor and the robot link. 

Variable Impedance Actuators The impedance of a joint is typically con¬ 
trolled using a feedback control law, as described in Section 11.5. There are lim¬ 
its to the bandwidth of this control, however; a joint that is actively controlled 
to behave as a spring will only achieve spring-like behavior to low-frequency 
perturbations. 

A new class of actuators, called variable impedance actuators or vari¬ 
able stiffness actuators, aims to give actuators the desired passive mechanical 
impedance without the bandwidth limitations of an active control law. As an 
example, a variable stiffness actuator may consist of two motors, with one mo¬ 
tor independently controlling the mechanical stiffness of the joint (e.g., based 
on the setpoint of an internal nonlinear spring) while the other motor produces 
a torque. 


11.7 Summary 


• A PID joint-space feedback controller takes the form 



where 6 e = Od — 0 and 9d is the vector of desired joint angles. The pro¬ 
portional term acts like a virtual spring, to pull the robot joints to the 
desired positions; the derivative term acts like a virtual damper, acting to 
reduce velocity differences between the actual and desired velocities; and 
the integral term can be used to eliminate steady-state error in setpoint 
control. 

• The linear error dynamics 


T On— l^e™ ^ + • • • + d2@e T d\9 e T OQ^e — 0 


are stable, i.e., initial errors converge exponentially to zero, if and only if 
all of the complex roots si,..., s n of the characteristic equation 




+ ... + 0 . 2 ^ T cqs T uq — 0 


have real components less than zero, i.e., Re(si) < 0 for all i = 1... n. 
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• Stable second-order linear error dynamics can be written in the standard 
form 

9 e + 2 £ o J n 9 e + = 0 , 

where £ is the damping ratio and Lo n is the natural frequency. The roots 
of the characteristic equation are 

Si,2 = -C^n ± Wn\/C 2 “ 1- 

The system is overdamped if £ > 1, critically damped if £ = 1, and 
underdamped if £ < 1. The step response of a stable second-order system 
is often characterized by its overshoot, 2% settling time, and steady-state 
error. 

• The joint-space computed torque controller is 

r = M ( 9 ) ^9 d + K p 9 e + Ki J 9 e (t)dt + K d 9 e ^ + h(9,9). 

This controller cancels nonlinear terms, uses feedforward control to antic¬ 
ipate the desired acceleration, and uses linear feedback control for stabi¬ 
lization. 

• For robots without joint friction and a perfect model of gravity forces, 
joint-space PD setpoint control plus gravity compensation 

r = K p 9 e + K d 9 + g{9) 

yields global convergence to 9 e = 0 by the Krasovskii-LaSalle invariance 
principle. 

• The task-space computed torque motion controller is 

r = J t (9) (a (9) + K p X e + K t J X e (t)dt + K d v)j + fj(9, V)) . 

There are several possible choices for calculating X e and V e . 

• Task-space force control can be achieved by the controller 

r = g{9) + J t ( 9) (x d + K f pT e + K fi j T e (t)dt - K damp V^j , 

consisting of gravity compensation, feedforward force control, PI force 
feedback, and damping to prevent fast motion. 

• In an m-dimensional task space (where typically m = 6), rigid constraints 
specify m — k free motion directions and k constraint directions in which 
forces can be applied. These constraints can be represented as A(X)V = 0. 
A force T can be partitioned as T = P(X)J r + (I — P(X))J r , where P(X) 
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projects to forces that move the end-effector tangent to the constraints 
and I — P(X) projects to forces against the constraints. The projection 
matrix P(X) is written in terms of the task-space mass matrix A(0) and 
constraints A(X) as 

P = I- A t (AA- 1 A t )- 1 AA~ 1 . 


• A hybrid motion-force controller using the projection P{X) can be written 
r = J T (0) ( P(X) ( A (9) V d + K p X e + Ki f X e (t)dt + K d V e 


motion control 


+ (/ - P{X)) (p d + K fp P e + K fi J Pe(t)d?j 


force control 

rj{6,V) 

nonlinear compensation 


• An impedance controller measures end-effector motions and creates end¬ 
point forces to mimic a mass-spring-damper system. An admittance con¬ 
troller measures end-effector forces and creates endpoint motions to achieve 
the same purpose. 


11.8 Software 


taulist = ComputedTorque(thetalist,dthetalist,eint,g, 

Mlist,Glist,Slist,thetalistd,dthetalistd,ddthetalistd,Kp,Ki,Kd) 

This function computes the joint controls r for the computed torque control 
law (11.21) at a particular time instant. The inputs are the n-vectors of joint 
variables, joint velocities, and joint error integrals; the gravity vector g; a list 
of transforms AA-m describing the link center of mass locations; a list of link 
spatial inertia matrices Qf, a list of joint screw axes Si expressed in the base 
frame; the n-vectors 9 d , 9 d , and 9 d describing the desired motion; and the scalar 
PID gains k p , ki , and k dl where the gain matrices are just K p = k p I , Ki = kil , 
and K d = k d I. 

[taumat,thetamat] = SimulateControl(thetalist,dthetalist,g, 

Ftipmat,Mlist,Glist,Slist,thetamatd,dthetamatd,ddthetamatd, 
gtilde.Mtildelist.Gtildelist,Kp,Ki,Kd,dt,intRes) 

This function simulates the performance of the computed torque control law (11.21) 
over a given desired trajectory. Inputs include the initial state of the robot, 0(0) 
and 0(0); the gravity vector g; an N x 6 matrix of wrenches applied by the end- 
effector, where each of the N rows is an instant in time in the trajectory; a list 
of transforms Mj _a describing the link center of mass locations; a list of link 
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spatial inertia matrices Qf, a list of joint screw axes Si expressed in the base 
frame; the N xn matrices of desired joint positions, velocities, and accelerations, 
where each of the N rows corresponds to an instant in time; a (possibly incor¬ 
rect) model of the gravity vector; a (possibly incorrect) model of the transforms 
M,_i a (possibly incorrect) model of the link inertia matrices; the scalar PID 
gains k p , ki , and kd , where the gain matrices are just K p = k p I , Ki = kil, and 
Kd = kdl ; the timestep between each of the N rows in the matrices defining 
the desired trajectory; and the number of integration steps to take during each 
timestep. 

11.9 Notes and References 

The computed torque controller originates from research in the 1970’s [98, 79, 
7, 103], and issues with its practical implementation (e.g., its computational 
complexity and modeling errors) have driven much of the subsequent research 
in nonlinear control, robust control, iterative learning control, and adaptive 
control. PD control plus gravity compensation was suggested and analyzed 
in [134], and subsequent analysis and modification of the basic controller is 
reviewed in [50]. 

The task space approach to motion control, also called operational space 
control, was originally outlined in [72, 52]. A geometric approach to tracking 
control for mechanical systems is presented in [15], where the configuration space 
for the system can be a generic manifold, including SO( 3) and SE( 3). 

The notion of natural and artificial constraints in hybrid motion-force control 
was first described by Mason [80], and an early hybrid motion-force controller 
based on these concepts is reported in [102]. As pointed out by Duffy [28], we 
must take care in specifying the subspaces in which motions and forces can be 
controlled. The approach to hybrid motion-force control in this chapter mirrors 
the geometric approach of Liu and Li [67]. Impedance control was first described 
in a series of papers by Hogan [38, 39, 40]. 

Robot control builds on the well-established field of linear control (e.g., 
[33, 3]) and the growing field of nonlinear control. General references on robot 
control include the edited volume [23] and the textbooks by Spong et al. [129], 
Siciliano et al. [124], Craig [22], Murray et al. [90], the Handbook of Robotics 
chapters on Motion Control [20] and Force Control [137], the Robot Motion 
Control chapter in the Encyclopedia of Systems and Control [128], and, for 
underactuated and nonholonomic robots, the chapters in the Control Hand¬ 
book [74] and the Encyclopedia of Systems and Control [73]. 
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11.10 Exercises 

1. Classify the following robot tasks as motion control, force control, hybrid 
motion-force control, impedance control, or some combination. Justify your 
answer. 

(i) Tightening a screw with a screwdriver. 

(ii) Pushing a box on the floor. 

(iii) Pouring a glass of water. 

(iv) Shaking hands with a human. 

(v) Throwing a baseball to hit a target. 

(vi) Shoveling snow. 

(vii) Digging a hole. 

(viii) Giving a back massage. 

(ix) Vacuuming the floor. 

(x) Carrying a tray of glasses. 

2. The 2% settling time of an underdamped second-order system is approxi¬ 
mately t = 4/(£w n ), based on e - ^"* = 0.02. What is the 5% settling time? 

3. Solve for any constants and give the specific equation for an underdamped 
second-order system with uj n = 4, £ = 0.2, 0 e (O) = 1, and 9 e ( 0) = 0 (a step 
response). Calculate the damped natural frequency, approximate overshoot, 
and 2% settling time. Plot the solution on a computer and measure the exact 
overshoot and settling time. 

4. Solve for any constants and give the specific equation for an underdamped 
second-order system with ui n = 10, £ = 0.1, 9 e ( 0) = 0, and 0 e (O) = 1. Calculate 
the damped natural frequency. Plot the solution on a computer. 

5. Consider a pendulum in gravity g = 10 m/s 2 . The pendulum consists of a 
2 kg mass at the end of a 1 m massless rod. The pendulum joint has a viscous 
friction coefficient of b = 0.1 Nms/rad. 

(i) Write the equation of motion of the pendulum in terms of 9, where 9 = 0 
corresponds to the “hanging down” configuration. 
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(ii) Linearize the equation of motion about the stable “hanging down” equi¬ 
librium. To do this, replace any trigonometric terms in 0 with the linear 
term in the Taylor expansion. Give the effective mass and spring con¬ 
stants to and k in the linearized dynamics m 9 + bd + kO = 0. At the stable 
equilibrium, what is the damping ratio? Is the system underdamped, crit¬ 
ically damped, or overdamped? If it is underdamped, what is the damped 
natural frequency? What is the time constant of convergence to the equi¬ 
librium and the 2% settling time? 

(iii) Now write the linearized equations of motion for 0 — 0 at the balanced 
upright configuration. What is the effective spring constant fc? 

(iv) You add a motor at the joint of the pendulum to stabilize the upright 
position, and you choose a P controller r = K p 0. For what values of K p 
is the upright position stable? 


6. You will develop a controller for a one-degree-of-freedom mass-spring-damper 
system of the form mi + bx + kx = /, where / is the control force and m = 4 kg, 
b = 2 Ns/m, and k = 0.1 N/m. 

(i) What is the damping ratio of the uncontrolled system? Is the uncon¬ 

trolled system overdamped, underdamped, or critically damped? If it is 
underdamped, what is the damped natural frequency? What is the time 
constant of convergence to the origin? 

(ii) Choose a P controller / = K p x e , where x e = Xd — x is the position error 

and Xd — 0. What value of K p yields critical damping? 

(iii) Choose a D controller / = K d x e , where x d = 0. What value of K d yields 

critical damping? 

(iv) Choose a PD controller that yields critical damping and a 2% settling time 
of 0.01 s. 

(v) For the PD controller above, if x d = 1 and x d = x d = 0, what is the 
steady-state error x e (t) as t goes to infinity? What is the steady-state 
control force? 

(vi) Now plug a PID controller in for /. Assume x d ^ 0 and x d = x d = 0. 
Write the error dynamics in terms of x e , x e , x e , and f x e (t)dt on the 
left-hand side and a constant forcing term on the right-hand side. (Hint: 
You can write kx as —k(x d — x) + kx d .) Take the time derivative of this 
equation and give the conditions on K p , Ki 1 and K d for stability. Show 
that zero steady-state error is possible with a PID controller. 


7. Simulation of a one-degree-of-freedom robot and robot controller. 
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(i) Write a simulator for a one-joint robot consisting of a motor rotating a 
link in gravity using the model parameters given in Section 11.2.2. The 
simulator should consist of (1) a dynamics function that takes as input the 
current state of the robot and the torque applied by the motor and gives as 
output the acceleration of the robot; and (2) a numerical integrator that 
uses the dynamics function to calculate the new state of the system over a 
series of timesteps At. A first-order Euler integration method suffices for 
this problem (e.g., 9{k + 1) = 9(k) + 9(k)At, 9(k + 1) = 9(k) + 9(k)At). 
Test the simulator in two ways: (a) starting the robot at rest at 9 = —7r/2 
and applying a constant torque of 0.5 Nm and (b) starting the robot at 
rest at 9 = — 7t/4 and applying a constant torque of 0 Nm. For both 
examples, plot the position as a function of time for sufficient duration to 
see the basic behavior. Make sure the behavior makes sense. A reasonable 
choice of At is 1 ms. 

(ii) Add two more functions to your simulator: (1) a trajectory generator 
function that takes the current time and returns the desired state and 
acceleration of the robot, and (2) a control function that takes the cur¬ 
rent state of the robot and information from the trajectory generator and 
returns a control torque. The simplest trajectory generator would return 
9 = 9d\ and 9 = 9 = 0 for all time t < T, and 9 = 9d2 ^ 9di and 9 = 9 = 0 
for all time t > T. This trajectory is a step function in position. Use PD 
feedback control for the control function and set K p = 10 Nm/rad. For a 
well-tuned choice of Kd , give Kd (including units) and plot the position as 
a function of time over 2 seconds for an initial state at rest at 9 = —7r/2 
and a step trajectory with 9di = — 7t/2 and 9d2 = 0. The step occurs at 
T = Is. 

(iii) Demonstrate two different choices of Kd that yield (1) overshoot and (2) 
sluggish response with no overshoot. Give the gains and the position plots. 

(iv) Add a nonzero Ki to your original well-tuned PD controller to eliminate 
steady-state error. Give the PID gains and plot the results of the step 
test. 


8. Modify the simulation of the one-joint robot in Exercise 7 to model a flexible 
transmission from the motor to the link with a stiffness of 500 Nm/rad. Tune a 
PID controller to give a good step response from 9 = —7r/2 to 9 = 0. Give the 
gains and plot the response. 

9. Simulation of a two-degree-of-freedom robot and robot controller (Fig¬ 
ure 11.17). 

(i) Dynamics. Derive the dynamics of a 2R robot in gravity (Figure 11.17). 
The mass of link i is rrq, the center of mass is a distance from the joint, 
the scalar inertia of link i about the joint is It, and the length of link i is 
Li . There is no friction at the joints. 
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(ii) Direct drive. Assume each joint is directly driven by a DC motor with 
no gearing. Each motor comes with specifications of the mass mf tator 
and inertia X I stator of the stator and the mass m- otor and inertia X™ tor of 
the rotor (the spinning portion). For the motor at joint i. the stator is 
attached to link i — 1 and the rotor is attached to link i. The links are 
thin uniform-density rods of mass m, and length Li. 

In terms of the quantities given above, for each link i £ {1, 2} give equa¬ 
tions for the total inertia Li about the joint, mass lrq, and distance r, from 
the joint to the center of mass. Think about how to assign the mass and 
inertia of the motors to the different links. 

(iii) Geared robot. Assume motor i has gearing with gear ratio Gi, and that the 
gearing itself is massless. As in the part above, for each link i £ {1,2}, give 
equations for the total inertia Li about the joint, mass , and distance 
ri from the joint to the center of mass. 

(iv) Simulation and control. As in Exercise 7, write a simulator with (at 
least) four functions: a dynamics function, a numerical integrator, a tra¬ 
jectory generator, and a controller. Assume zero friction at the joints, 
g = 9.81 m/s 2 in the direction indicated, Li = 1 m, r* = 0.5 m, mi = 3 kg, 
m 2 = 2 kg, L\ = 2 kgm 2 , and L 2 = 1 kgm 2 . Write a PID controller, find 
gains that give a good response, and plot the joint angles as a function of 
time for a trajectory with a reference at rest at (6 i,6 2 ) = (—7r/2, 0) for 
t < Is and ( 61 , 62 ) = (0, —7t/ 2) for t > 1 s. The initial state of the robot 
is at rest with ( 61 ,9 2 ) = (—7r/2, 0). 

(v) Torque limits. Real motors have limits on torque. While these limits are 
generally velocity dependent, here we assume that each motor’s torque 
limit is independent of velocity, < |r™ ax |. Assume r™ ax = 100 Nm 
and r™ ax = 20 Nm. If the control law requests greater torque, the ac¬ 
tual torque is saturated to these values. Rerun the previous PID control 
simulation and plot the torques as well as the position as a function of 
time. 

(vi) Friction. Add a viscous friction coefficient of bi = 1 Nms/rad to each joint 
and rerun the previous PID control simulation. 


10 . For the two-joint robot of Exercise 9, write a more sophisticated trajectory 
generator function. The trajectory generator should take as input 

• the desired initial position, velocity, and acceleration of each joint; 

• the desired final position, velocity, and acceleration; and 

• the total time of motion T. 


A call of the form 
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Figure 11.17: A two-link robot arm. The length of link i is Li and its inertia 
about the joint is Li. Gravity is g = 9.81 m/s 2 . 


[qd,qdotd,qdotdotd] = trajectory(time) 

returns the desired position, velocity, and acceleration of each joint at time 
time. The trajectory generator should provide a trajectory that is a smooth 
function of time. 

As an example, each joint could follow a fifth-order polynomial trajectory of 
the form 

QdiC) = T o.\t T 0 , 2 1 T T 04 1 T 0 , 5 b . (11.53) 

Given the desired positions, velocities, and accelerations of the joints at times 
t = 0 and t = T, you can uniquely solve for the six coefficients ao .. . 0,5 by 
evaluating Equation (11.53) and its first and second derivatives at t = 0 and 
t = T. 

Tune a PID controller to track a fifth-order polynomial trajectory moving 
from rest at (6b, O 2 ) = (—7r/2,0) to rest at (# 1 , 0 2 ) = (0, — 7t/2) in T = 2 s. Give 
the values of your gains and plot the reference position of both joints and the 
actual position of both joints. You are free to ignore torque limits and friction. 

11 . For the two-joint robot of Exercise 9 and fifth-order polynomial trajectory 
of Exercise 10, simulate a computed torque controller to stabilize the trajectory. 
The robot has no joint friction or torque limits. The model of the link masses 
should be 20% greater than their actual values to create error in the feedforward 
model. Give the PID gains and plot the reference and actual joint angles for 
both the computed torque controller as well as PID control only. 

12 . The Krasovskii-LaSalle invariance principle states the following: Given a 
system x = f(x),x € 1™ such that /(0) = 0, and any energy-like function V(x) 
such that 


V(x) > 0 for all x Q; 
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• V ( x ) ->ooasi-> oo; 

• 1^(0) = 1>(0) = 0; and 

• V{x) < 0 along all trajectories of the system, 

let S be the largest set of M™ such that V(x) =0 and trajectories beginning 
in S remain in S for all time. Then if S contains only the origin, the origin is 
globally asymptotically stable—all trajectories converge to the origin. 

Show how the Krasovskii-LaSalle principle is violated for centralized multi¬ 
joint PD setpoint control with gravity compensation, using the energy function 
V(x) from Equation (11.26), if K p = 0 or Kd = 0. For a practical robot system, 
is it possible to use the Krasovskii-LaSalle invariance principle to demonstrate 
global asymptotic stability even if Kd = 0? Explain your answer. 

13 . The two-joint robot of Exercise 9 can be controlled in task space using the 
endpoint task coordinates X = (x, y), as shown in Figure 11.17. The task space 
velocity is V = X. Give the Jacobian J{6) and the task-space dynamics model 
{A(0),rJ(Q,V)} in the computed torque control law (11.33). 

14 . Choose appropriate space and end-effector reference frames {s} and {b} 
and express natural and artificial constraints, six each, that achieve the following 
tasks: (a) opening a cabinet door; (b) turning a screw that advances linearly a 
distance p for every revolution; and (c) drawing a circle on a chalkboard with a 
piece of chalk. 

15 . Assume the end-effector of the two-joint robot in Figure 11.17 is constrained 
to move on the line x — y = 1. The robot’s link lengths are L\ = L 2 = 1. (a) 
Write the constraint as A(X)V = 0, where X = (x,y) and V = X. (b) Write 
the constraint as A(9)9 = 0. 

16 . Derive the constrained motion equations (11.45) and (11.46). Show all the 
steps. 

17 . We have been assuming that each actuator delivers the torque requested by 
the control law. In fact, there is typically an inner control loop running at each 
actuator, typically at a higher servo rate than the outer loop, to try to track 
the torque requested. Figure 11.18 shows two possibilities for a DC electric 
motor, where the torque r delivered by the motor is proportional to the current 
I through the motor, r = ktl. The torque from the DC motor is amplified by 
the gearhead with gear ratio G. 

In one control scheme, the motor current is measured by a current sensor and 
compared to the desired current I co m ; the error is passed through a PI controller 
which sets the duty cycle of a low-power pulse-width-modulation (PWM) digital 
signal; and the PWM signal is sent to an H-bridge that generates the actual 
motor current. In the second scheme, a strain gauge torque sensor is inserted 


398 


Robot Control 



gauge 


link 


link 


Figure 11.18: Two methods for controlling the torque at a joint driven by a 
geared DC motor. (Top) The current to the motor is measured by measuring 
the voltage across a small resistance in the current path. A PI controller works 
to make the actual current better match the requested current J com . (Bottom) 
The actual torque delivered to the link is measured by a strain gauge. 


between the output of the motor gearing and the link, and the measured torque 
is compared directly to the requested torque T CO m- Since a strain gauge measures 
deflection, the element it is mounted on must have a finite torsional stiffness. 
Series elastic actuators are designed to have particularly flexible torsional ele¬ 
ments, so much so that encoders are used to measure the larger deflection. The 
torque is estimated from the encoder reading and the torsional spring constant. 

(i) For the current sensing scheme, what multiplicative factor should go in 
the block labeled /com/fcom? Even if the PI current controller does its job 
perfectly (/error = 0) and the torque constant k t is perfectly known, what 
effect may contribute to error in the generated torque? 

(ii) For the strain gauge measurement method, explain the drawbacks, if any, 
of having a flexible element between the gearhead and the link. 


18. Modify the SimulateControl function to allow initial state errors. 










































Chapter 12 


Grasping 


and Manipulation 


Most of the book so far has been concerned with kinematics, dynamics, motion 
planning, and control of the robot itself. Only in Chapter 11, on the topics 
of force control and impedance control, did the robot finally begin interacting 
with an environment other than free space. This is when a robot really becomes 
valuable—when it can perform useful work on objects in the environment. 

In this chapter our focus moves outward, from the robot itself to the interac¬ 
tion between the robot and its environment. The desired behavior of the robot 
hand or end-effector, whether motion control, force control, hybrid motion-force 
control, or impedance control, is assumed to be achieved perfectly using the 
methods discussed so far. Our focus is on the contact interface between the 
robot and objects, as well as contacts among objects and between objects and 
constraints in the environment. In short, our focus is on manipulation rather 
than the manipulator. Examples of manipulation include grasping, pushing, 
rolling, throwing, catching, tapping, etc. To limit our scope, we will assume 
that the manipulator, objects, and obstacles in the environment are rigid. 

To simulate, plan, and control robotic manipulation tasks, we need an under¬ 
standing of (at least) three elements: contact kinematics; forces applied through 
contacts; and the dynamics of rigid bodies. Contact kinematics studies how rigid 
bodies can move relative to each other without penetration, and classifies these 
feasible motions according to whether the contacts are rolling, sliding, or sepa¬ 
rating. Contact force models address the normal and frictional forces that can 
be transmitted through rolling and sliding contacts. Finally, the actual motions 
of the bodies are those that simultaneously satisfy the kinematic constraints, 
contact force model, and rigid-body dynamics. The environment is assumed to 
consist of one or more rigid movable parts, and perhaps stationary constraints 
such as floors, walls, and fixtures. The manipulator consists of one or more 
controlled bodies (e.g., “fingers”) which could be under motion, force, hybrid 
motion-force, or impedance control. 

The following definitions from linear algebra will be useful in this chapter: 
Definition 12.1. Given a set of j vectors A = or,... aj £ K n , we define the 
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(a) 


(b) 


(c) 


(d) 


Figure 12.1: (a) Three vectors in R 2 , drawn as arrows from the origin, (b) The 
linear span of the vectors is the entire plane, (c) The positive linear span is the 
cone shaded gray, (d) The convex span is the polygon and its interior. 

linear span, or the set of linear combinations, of the vectors to be 



the nonnegative linear combinations, sometimes called the positive span, 

to be 



and the convex span to be 



Clearly conv(^4) C pos(.A) C span(.A) (see Figure 12.1). The following facts 
from linear algebra will also be useful. 

1. The space R n can be linearly spanned by n vectors, but no fewer. 

2. The space R n can be positively spanned by n + 1 vectors, but no fewer. 

The first fact is implicit in our use of n coordinates to represent n-dimensional 
Euclidean space. Fact 2 follows from the fact that for any choice of n vectors 
A = {ai,... ,a n }, there exists a vector c£l" such that ajc < 0 for all i. In 
other words, no nonnegative combination of vectors in A can create a vector 
in the direction c. On the other hand, if we choose a \,..., a n to be orthogonal 
coordinate bases of R", then choose a n+ 1 = — X]"=i we see that this set of 
n + 1 vectors positively spans R". 


12.1 Contact Kinematics 


Contact kinematics is the study of how two or more rigid bodies can move 
relative to each other while respecting the impenetrability constraint. It also 
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classifies motion in contact as either rolling or sliding. Let’s start by looking at 
a single contact between two rigid bodies. 

12.1.1 First-Order Analysis of a Single Contact 

Consider two rigid bodies whose configuration is given by the local coordinate 
column vectors q± and q 2 , respectively. Writing the composite configuration 
as q = we define a distance function d(q) between the parts that is 

positive when they are separated, zero when they are touching, and negative 
when they are in penetration. When d(q) > 0, there are no constraints on the 
motions of the parts; each is free to move with six degrees of freedom. When 
the parts are in contact (d(q) = 0), we look at the time derivatives d, d, etc., to 
determine if the parts stay in contact or break apart as they follow a particular 
trajectory q(t). This can be determined by the following table of possibilities: 


d 

> 0 
< 0 

d 

d 

no contact 

infeasible (penetration) 

= 0 

> 0 


in contact, but breaking free 

= 0 

< 0 


infeasible (penetration) 

= 0 

= 0 

> 0 

in contact, but breaking free 

= 0 

etc. 

= 0 

< 0 

infeasible (penetration) 


The contact is maintained only if all time derivatives are zero. 

Now let’s assume that the two bodies are initially in contact (d 
single point. The first two time derivatives of d are written 


; dd. 
d = 

dq 

■■ , T d 2 d. dd.. 
d = q ~K~q. 

Oq 1 oq 


0) at a 


( 12 . 1 ) 

( 12 . 2 ) 


The terms dd/dq and d 2 d/dq 2 carry information about the local contact geom¬ 
etry. The gradient vector dd/dq corresponds to the separation direction in q 
space associated with the contact normal (Figure 12.2). The matrix d 2 d/dq 2 
encodes information about the relative curvature of the parts at the contact 
point. 

In this chapter we assume that only contact normal information dd/dq is 
available at contacts. Other information about the local contact geometry, in¬ 
cluding the contact curvature d 2 d/dq 2 and higher derivatives, is unknown. With 
this assumption, we truncate our analysis at Equation (12.1) and assume bod¬ 
ies remain in contact if d = 0. Since we deal only with the first-order contact 
derivative dd/dq , we refer to our analysis as a first-order analysis. By a first- 
order analysis, the contacts in Figure 12.2 are treated identically, since they 
have the same contact normal. 
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Figure 12.2: (Left) The parts A and B in single-point contact define a contact 
tangent plane and a contact normal vector n perpendicular to the tangent plane. 
By default, the positive direction of the normal is chosen into body A. Since 
contact curvature is not addressed in this chapter, the contact places the same 
restrictions on the motions of the rigid bodies in the middle and right panels. 


As indicated in the table above, a second-order analysis incorporating con¬ 
tact curvature d 2 d/dq 2 may indicate that the contact is actually breaking or 
penetrating even when d = d = 0. We will see examples of this, but a detailed 
analysis of second-order contact conditions is beyond the scope of this chapter. 

12.1.2 Contact Types: Rolling, Sliding, and Breaking Free 

Given two bodies in single-point contact, they undergo a roll-slide motion 
if the contact is maintained. The constraint that contact is maintained is a 
holonomic constraint. A necessary condition for maintaining contact is d = 0. 

Let’s write the velocity constraint d = 0 in a form that does not require 
an explicit distance function, based on the contact normal (Figure 12.2). Let 
n £ K 3 be a unit vector aligned with the contact normal, expressed in a world 
frame. Let pa £ K 3 be a representation of the point in contact on part A in the 
world frame, and p B £ R 3 be the point in contact on part B. The condition 
d = 0 is written 

h T (pA-PB) = 0. (12-3) 

Since the contact normal is defined as into body A, the impenetrability con¬ 
straint d > 0 is written 

h T (pA-PB)> 0. (12.4) 

Let us rewrite the constraint (12.4) in terms of the twists Va = (wa, va) and 
Vb = (wg,!)s) of parts A and B in a space frame, respectively. 1 Note that 

PA = VA +COA x PA = VA + [ua]PA 
Pb = V B + WB X p B = V B + [ lo b \pb■ 

1 A11 twists and wrenches are expressed in a space frame in this chapter. 
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We can also write the wrench T = (m, f) corresponding to a unit force applied 
along the contact normal: 

T = (p A x n,n) = (\p A \n,h). 

While it is not necessary to appeal to forces in a purely kinematic analysis of 
rigid bodies, we will find it convenient to adopt this notation now in anticipation 
of contact forces in Section 12.2. 

With these expressions, the inequality constraint (12.4) can be written 

(impenetrability constraint) T t (Va — V B ) > 0. (12-5) 

(See Exercise 1). If 

(active constraint) F t {Va — V B ) = 0, (12-6) 

then, to first order, the constraint is active and the parts remain in contact. 

In the case that B is a stationary fixture, the impenetrability constraint 
(12.5) simplifies to 

T t V a > 0. (12.7) 

If the condition (12.7) is satisfied, T and Va are said to be repelling. If 
T t Va = 0, T and Va are said to be reciprocal and the constraint is active. 

Twists Va and V B satisfying (12.6) are called first-order roll-slide mo¬ 
tions -the contact may be either sliding or rolling. Roll-slide contacts may 
be further separated into rolling contacts and sliding contacts. The contact 
is rolling if the parts have no motion relative to each other at the contact: 

(rolling constraint) p A = v A + [oja]pa = v B + [v b ]pb = Pb ■ (12.8) 

Note that “rolling” contacts include those where the two parts remain stationary 
relative to each other, i.e., no relative rotation. Thus “sticking” is another term 
for these contacts. 

If the twists satisfy Equation (12.6) but not the rolling equations of (12.8), 
then they are sliding. 

We assign a rolling contact the contact label R, a sliding contact the label 
S, and a contact that is breaking free (the impenetrability constraint (12.5) is 
satisfied but not the active constraint (12.6)) the label B. 

The distinction between rolling and sliding contacts becomes especially im¬ 
portant when we consider friction forces in Section 12.2. 

Example 12 . 1 . Consider the contact shown in Figure 12.3. Parts A and B are 
in contact at p A = Pb = (1, 2, 0) T with contact normal direction n = (0, 1, 0) T . 
The impenetrability constraint (12.5) is 

F T (V a -V b )>0 

ua-ub > 

VA ~ V B \ ~ 

[0, 0, 1, 0, 1, 0] ^Ay ^Byi ^Az ^Bzi 

^Ax ^Bxi ^Ay ^Byi VAz ^Bz] ^ 0 

WAz ~ WBz + VAy ~ VBy > 0, 


[([ Va\u) T n T ] 




404 


Grasping and Manipulation 



Figure 12.3: Example 12.1. (Left) The body B makes contact with A at pa = 
Pb = (1,2,0) T with normal h = (0,1, 0) T . (Middle) The velocities Va and their 
corresponding contact labels for B stationary and A confined to a plane. The 
contact normal force T is (m x , m y , m z , f x , f y , f z ) T = (0, 0,1, 0,1, 0) T . (Right) 
Looking down the —vax axis. 


and therefore roll-slide twists satisfy 

VAz - V B z + VAy - V B y = 0. (12.9) 

Equation (12.9) defines an 11-dimensional hyperplane in the 12-dimensional 
space of twists (V 4 , Vb). 

The rolling constraints (12.8) are 

VAx + UAzPAy — V AyP Az = v Bx + VBzPBy ~ VByPBz 

VAy + VAzPAx ~ VAxPAz = VBy + VBzPBx ~ VBxPBz 

VAz 4” VAxPAy V AyP Ax = VBz T VBxPBy VByPBxi 

and plugging in for pa and ps, we get 

v Ax + 2 u A z = v Bx + 2 w Sz (12.10) 

VAy + UAz=V B y+UBz (12.11) 

VAz + 2 V Ax - VAy = VBz + ^V B x ~ V B y (12.12) 

The constraint equations (12.10)-(12.12) define a 9-dimensional hyperplane sub¬ 
set of the 11 -dimensional hyperplane of roll-slide twists. 

To visualize the constraints in a low-dimensional space, let’s assume that 
B is stationary (Vb = 0) and A is confined to the z = 0 plane, i.e., Va = 
{uAx,UAy,UAz,VAx,v Ay ,VAz) T = (0, 0, u A z, v Ax , v Ay , 0) T . The wrench T is 
written (m z , f x , f y ) T = (1,0,1) T . The roll-slide constraint (12.9) reduces to 

VAy + VAz = 0, 

while the rolling constraints simplify to 


v Ax + 2 v Az = 0 

VAy + VAz = 0 
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The single roll-slide constraint yields a plane in the (iOAz,VAx,VAy) space, and 
the two rolling constraints yield a line in that plane. Because Vb = 0, the 
constraint surfaces pass through the origin Va = 0. If Vb ^ 0, this is no longer 
the case in general. 

Figure 12.3 graphically shows that nonpenetrating twists Va must have a 
nonnegative dot product with the constraint wrench T when Vb — 0. 

12.1.3 Multiple Contacts 

Now suppose that A is subject to several contacts, from B and perhaps other 
parts. Each impenetrability constraint (12.5) constrains Va to a half-space of 
its six-dimensional twist space bounded by a five-dimensional hyperplane of the 
form T t Va — -F T Vg. Unioning the set of constraints from all the contacts, we 
get a polyhedral convex set (polytope 2 for short) V of feasible twists in the 
Va space, written 

V = {V A \ Tj ( V A - Vi) > 0 V*}, 

where J) corresponds to the ith contact normal and Vi is the twist of the other 
part in contact at contact i. A contact constraint i is redundant if the half-space 
constraint contributed by T % does not change the feasible twist polytope V. In 
general, the feasible twist polytope for a part can consist of a six-dimensional 
interior (where no contact constraint is active), five-dimensional faces where one 
constraint is active, four-dimensional faces where two constraints are active, and 
so on, down to one-dimensional edges and zero-dimensional points. A twist Va 
on an n-dimensional facet of the polytope indicates that 6 — n independent 
(non-redundant) contact constraints are active. 

If all of the bodies providing constraints are stationary, such as fixtures, then 
each constraint hyperplane defined by (12.5) passes through the origin of the Va 
space. We call such a constraint homogeneous. The feasible twist set becomes 
a cone rooted at the origin, called a (homogeneous) polyhedral convex cone. 
Let T l be the constraint wrench of stationary contact i. Then the feasible twist 
cone V is 

V = {V A | Tj V A > 0 V*}. 

If the Ti positively span the six-dimensional wrench space, or, equivalently, the 
convex hull of the contains the origin in the interior, then the feasible twist 
polytope reduces to a point at the origin, the stationary contacts completely 
constrain the motion of the part, and we have form closure, discussed in more 
detail in Section 12.1.7. 

As mentioned in Section 12.1.2, each point contact i can be given a label 
corresponding to the type of contact: B if the contact is breaking, R if the 
contact is rolling, and S if the contact is sliding, i.e., (12.6) is satisfied but 
(12.8) is not. The contact mode for the entire system can be written as the 

2 We use the term “polytope” to refer generally to a convex set bounded by hyperplanes in 
an arbitrary vector space. The set need not be finite; it could be a cone with infinite volume. 
It could also be a point at the origin, or the null set if the constraints are incompatible with 
the rigid-body assumption. 
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concatenation of the contact labels at the contacts. Since we have three distinct 
contact labels, a system of bodies with k contacts can have a maximum of 3 k 
contact labels. Some of these contact modes may not be feasible, however, as 
their corresponding kinematic constraints may not be compatible. 

Example 12.2. Figure 12.4 shows triangular fingers contacting a hexagonal 
part A. To more easily visualize the contact constraints, the hexagon is re¬ 
stricted to translational motion in a plane only, so that its twist can be written 
Va = (0,0,0,VAa:,^Ay,0). In Figure 12.4(a), the single stationary finger creates 
a contact wrench F\ that can be drawn in the Va space. All feasible twists 
have a nonnegative component in the direction of T\. Roll-slide twists satisfy¬ 
ing J-JVa = 0 he on the constraint line. Since no rotations are allowed, the 
only twist yielding a rolling contact is Va = 0. In Figure 12.4(b), the union of 
the constraints due to two stationary fingers creates a cone of feasible twists. 
Figure 12.4(c) shows three fingers in contact, one of which is moving with twist 
V3. Because the moving finger has nonzero velocity, its constraint half-space is 
displaced from the origin by V3. The result is a closed polygon of feasible twists. 

Example 12.3. Figure 12.5 shows the contact normals of three stationary 
contacts with a planar part A , not shown. The part moves in a plane, so vaz = 
u>Ax = ua v = 0. In this example we do not distinguish between rolling and 
sliding motions, so the locations of the contacts along the normals are irrelevant. 
The three contact wrenches, written (m z , f x , f y ), are T\ = (0,1,—2), = 

(—1,0,1), and Tz = (1, 0,1), yielding the motion constraints 

v A y ~ 2 u Az > 0 

-VAx + UAz > 0 
V Ax + WAz > 0. 

These constraints describe a polyhedral convex cone of feasible twists rooted at 
the origin, as illustrated in Figure 12.5. 

12.1.4 Collections of Parts 

The discussion above can be generalized to find the feasible twists of multiple 
parts in contact. If parts i and j make contact at a point p, where n points into 
part i and J- = (\p]n,n), then their spatial twists V* and Vj must satisfy the 
constraint 

F T {Vi - Vj) > 0 (12.13) 

to avoid penetration. This is a homogeneous half-space constraint in the com¬ 
posite (Vj, Vj) twist space. In an assembly of multiple parts, each pairwise 
contact contributes another constraint in the composite twist space, and the 
result is a polyhedral convex cone of kinematically feasible twists rooted at the 
origin of the composite twist space. The contact mode for the entire assembly 
is the concatenation of the contact labels at each contact in the assembly. 

If there are parts whose motion is controlled, e.g., robot fingers, the con¬ 
straints on the motion of the remaining parts are no longer homogeneous. As a 
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Figure 12.4: Motion-controlled fingers contacting a hexagon that is constrained 
to translate in a plane only (Example 12.2). (a) A single stationary finger pro¬ 
vides a single half-space constraint on the hexagon’s twist Va ■ The feasible mo¬ 
tion half-space is shaded gray. The two-dimensional set of twists corresponding 
to breaking contact B, the one-dimensional set corresponding to sliding contact 
S, and the zero-dimensional set corresponding to rolling (fixed) contact R are 
shown, (b) The union of constraints from two stationary fingers creates a cone 
of feasible twists. This cone corresponds to four possible contact modes: RR, 
SB, BS, and BB. The contact label for the leftmost finger is given first, (c) Three 
fingers, one of which is moving with a linear velocity V 3 , create a closed poly¬ 
gon of feasible twists. There are seven possible contact modes corresponding to 
the feasible twists: a two-dimensional set where all contacts are breaking, three 
one-dimensional sets where one contact constraint is active, and three zero¬ 
dimensional sets where two contact constraints are active. Note that rolling 
contact at the moving finger is not feasible, since translation of the hexagon to 
“follow” the moving finger, as indicated by the o at the lower right of the figure, 
would violate one of the impenetrability constraints. If the third finger were 
stationary, the only feasible motion of the hexagon would be zero velocity, with 
contact mode RRR. 


result, the convex polyhedral set of feasible twists of the uncontrolled parts, in 
their composite twist space, is no longer a cone rooted at the origin. 

12.1.5 Other Types of Contacts 

We have been considering point contacts of the type shown in Figure 12.6(a), 
where at least one of the bodies in contact uniquely defines the contact normal. 
Figures 12.6(b)-(e) show other types of contact. The kinematic constraints pro¬ 
vided by the convex-concave vertex, line, and plane contacts of Figures 12.6(b)- 
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A^ 
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contact 3 





Figure 12.5: Example 12.3. (Left) The lines of force corresponding to three 
stationary contacts on a planar body. If we are only concerned with feasible 
motions, and do not distinguish between rolling and sliding, contacts anywhere 
along the lines, with the contact normals shown, are equivalent. (Right) The 
three constraint half-spaces define a polyhedral convex cone of feasible twists. 
In the figure, the cone is truncated at the plane va v = 2. The outer faces of the 
cone are shaded white and the inner faces are gray. Twists in the interior of the 
cone correspond to all contacts breaking, while twists on the faces of the cone 
correspond to one active constraint, and twists on one of the three edges of the 
cone correspond to two active constraints. 



Figure 12.6: (a) Vertex-face contact, (b) A convex vertex contacting a concave 
vertex can be treated as multiple point contacts, one at each face adjacent to 
the concave vertex. These faces define the contact normals, (c) A line contact 
can be treated as two point contacts at either end of the line, (d) A plane 
contact can be treated as point contacts at the corners of the convex hull of the 
contact area, (e) Convex vertex-vertex contact. This case is degenerate and not 
considered. 


(d) are, to first order, identical to those provided by finite collections of single¬ 
point contacts. Constraints provided by other points of contact are redundant. 
The degenerate case in Figure 12.6(e) is ignored, as there is no unique definition 
of a contact normal. 

The impenetrability constraint (12.5) derives from the fact that arbitrarily 
large contact forces can be applied in the normal direction to prevent penetra¬ 
tion. In Section 12.2, we will see that tangential forces may also be applied 
due to friction, and these forces may prevent slipping between two objects in 
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contact 

type 

planar 

constraints freedoms 

spatial 

constraints freedoms 

FPC 

PCWF 

SC 

1 2 

2 1 

2 1 

1 5 

3 3 

4 2 


Table 12.1: The number of constraints and freedoms allowed by a kinematic 
model of a frictionless point contact (FPC), a point contact with friction 
(PCWF), and a soft contact (SC). 


contact. Normal and tangential contact forces are subject to constraints: the 
normal force must be pushing into a part, not pulling, and the maximum friction 
force is proportional to the normal force. 

If we wish to apply a kinematic analysis that can approximate the effects of 
friction without explicitly modeling forces, we can define three purely kinematic 
models of point contacts: a frictionless point contact, a point contact with 
friction, and a soft contact, also called a soft-finger contact. A frictionless 
point contact enforces only the roll-slide constraint (12.5). A point contact with 
friction also enforces the rolling constraints (12.8), implicitly modeling friction 
sufficient to prevent slip at the contact. A soft contact enforces the rolling 
constraints (12.8) as well as one more constraint: the two bodies in contact 
may not spin relative to each other about the contact normal axis. This models 
deformation and the resulting friction moment resisting spin due to the nonzero 
contact area between the two bodies. For planar problems, a point contact with 
friction and a soft contact are identical. See Table 12.1. 

Similar to joints, which also specify the number of constraints and freedoms, 
these kinematic models of contact allow the application of Griibler’s formula 
to determine the number of degrees of freedom of a set of bodies in contact. 
Because these models do not encode the unilateral nature of contact (contacts 
can “push” but not “pull”), however, we do not use them in the remainder of 
the chapter. 

12.1.6 Planar Graphical Methods 

Planar problems allow the possibility of using graphical methods to visualize the 
feasible motions for a single body, since the space of twists is three-dimensional. 
An example planar twist cone is shown in Figure 12.5. Such a figure would be 
very difficult to draw for a system with more than three degrees of freedom. 

A convenient way to represent a planar twist V = ( u > z , v x ,v y ) is as a center 
of rotation (CoR) at (—v v /lu z ,v x /u} z ) plus the angular velocity oj z . The CoR 
is the point in the (projective) plane that remains stationary under the motion, 
i.e., the point where the screw axis intersects the plane. 3 In the case that the 

3 Note that the case = 0 must be treated with care, as it corresponds to a CoR at 
infinity. 
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Figure 12.7: Given the velocity of two points on the part, the lines normal to 
the velocities intersect at the CoR. The CoR shown is labeled + corresponding 
to the (counterclockwise) positive angular velocity of the part. 



Figure 12.8: Mapping a planar twist V to a CoR. The ray containing a vector 
V intersects either the plane of + CoRs at w z = 1, the plane of — CoRs at 
ui z = —1, or the circle of translation directions. 


speed of motion is immaterial, we may simply label the CoR with a +, —, or 
0 sign representing the direction of rotation (Figure 12.7). The mapping from 
planar twists to CoRs is illustrated in Figure 12.8, which shows that the space 
of CoRs consists of a plane of + CoRs (counterclockwise), a plane of — CoRs 
(clockwise), and a circle of translation directions. 

Given two distinct twists Vi and V 2 , the set of linear combinations of these 
velocities k\Vi + fc 2 V 2 , where k±,k 2 £ M, maps to the line of CoRs containing 
CoR(Vi) and CoR(V 2 ). Since k\ and can have either sign, if either u>i z or 
W 2 z is nonzero, the CoRs on this line can have either sign. If uj\ z = W 2 z = 0, 
then this set corresponds to the set of all translation directions. 

A more interesting case is when &i, > 0. Given two twists Vi and V 2 , the 
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Figure 12.9: The intersection of a twist cone with the unit twist sphere, and the 
representation of the cone as a set of CoRs. 



Figure 12.10: (a) Positive linear combination of two CoRs labeled +. (b) Posi¬ 
tive linear combination of a + CoR and a — CoR. (c) Positive linear combination 
of three + CoRs. (d) Positive linear combination of two + CoRs and a — CoR. 


nonnegative linear combination of these two velocities is written 

V = pos({V 1 ,V 2 }) = {AhVi + k 2 V 2 | fci ,k 2 > 0}, 

a planar twist cone rooted at the origin, with Vi and V2 defining the edges of 
the cone. If oj\ z and ui 2 z have the same sign, then the CoRs of their nonnegative 
linear combinations CoR(pos({Vi, V2})) all have that sign, and lie on the line 
segment between the two CoRs. If CoR(Vi) and CoR(V2) are labeled + and 
—, respectively, then CoR(pos({Vi, V2})) consists of the line containing the two 
CoRs, minus the segment between the CoRs. This set consists of a ray of 
CoRs labeled + attached to CoR(Vi), a ray of CoRs labeled — attached to 
CoR(V2), and a point at infinity labeled 0, corresponding to translation. This 
collection should be considered as a single line segment (though one passing 
through infinity), just like the first case. Figures 12.9 and 12.10 show examples 
of CoR regions corresponding to positive linear combinations of planar twists. 

The CoR representation of planar twists is particularly useful for represent¬ 
ing the feasible motions of one movable body in contact with stationary bodies. 
Since the constraints are stationary, as noted in Section 12.1.3, the feasible 
twists form a polyhedral convex cone rooted at the origin. Such a cone can be 
represented uniquely by a set of CoRs with +, —, and 0 labels. A general twist 
polytope, as generated by moving constraints, cannot be uniquely represented 
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Figure 12.11: The stationary triangle makes contact with a movable part. CoRs 
to the left of the contact normal are labeled +, to the right are labeled —, and 
on the normal labeled ±. Also given are the contact types for the CoRs. For 
points on the contact normal, the sign assigned to SI and Sr CoRs switches at 
the contact point. Three CORs and labels are illustrated. 


by a set of CoRs with +, —, and 0 labels. 

Given a contact between a stationary body and a movable body, we can plot 
the CoRs that do not violate the impenetrability constraint. Label all points 
on the contact normal ±, points to the left of the inward normal +, and points 
to the right —. All points labeled + can serve as CoRs with positive angular 
velocity for the movable body, and all points labeled — can serve as CoRs with 
negative angular velocity, without violating the first-order contact constraint. 
We can further assign contact labels to each CoR corresponding to the first- 
order conditions for breaking contact B, sliding contact S, and rolling contact 
R. For planar sliding, we subdivide the label S into two subclasses: Sr, where 
the moving body slips right relative to the fixed constraint, and SI, where the 
moving body slips to the left. Figure 12.11 illustrates the labeling. 

If there is more than one contact, we simply union the constraints and con¬ 
tact labels from the individual contacts. The unioning of the constraints implies 
that the feasible CoR region is convex, as is the homogeneous polyhedral twist 
cone. 

Example 12.4. Figure 12.12(a) shows a planar part standing on a table while 
being contacted by a stationary robot finger. The finger defines an inequality 
constraint on the part’s motion and the table defines two more. The cone of 
twists that do not violate the impenetrability constraints is represented by the 
CoRs that are consistently labeled for each contact (Figure 12.12(b)). Each 
feasible CoR is labeled with a contact mode that concatenates the labels for the 
individual contacts (Figure 12.12(c)). 

Now look more closely at the CoR labeled (+, SrBSr) in Figure 12.12(c). 
Is this motion really possible? It should be apparent that it is, in fact, not 
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T 1 T 2 


(a) 


tS1S1B 

BS1B at infinity 



S1S1B 

i at infinity 


(b) (c) 


Figure 12.12: Example 12.4. (a) A part resting on a table, with two contact 
constraints provided by the table and a single contact constraint provided by 
the stationary finger, (b) The feasible twists representated as CoRs, shown in 
gray. Note that the lines that extend off to the left and to the bottom “wrap 
around” at infinity and come back in from the right and the top, respectively, so 
this CoR region should be interpreted as a single connected convex region, (c) 
The contact modes assigned to each feasible motion. The zero velocity contact 
mode is RRR. 


possible: the part would immediately penetrate the stationary finger. Our 
incorrect conclusion is due to the fact that our first-order analysis ignores the 
local contact curvature. A second-order analysis would show that this motion 
is impossible. On the other hand, if the radius of curvature of the part at the 
contact were sufficiently small, then the motion would be possible. 

Thus a first-order analysis of a contact indicating roll-slide motion might be 
classified as penetrating or breaking by a second-order analysis. Similarly, if 
our second-order analysis indicates a roll-slide motion, a third or higher-order 
analysis may indicate penetration or breaking free. In any case, if an nth-order 
analysis indicates that the contact is breaking or penetrating, then no analysis 
of order greater than n will change the conclusion. 

12.1.7 Form Closure 

Form closure of an object is achieved if a set of stationary constraints prevents 
all motion of the object. If these constraints are robot fingers, we call this a 
form-closure grasp. An example is shown in Figure 12.13. 
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Figure 12.13: (Left) The part from Figure 12.12, with three stationary point 
contacts and the part’s feasible twist cone represented as a convex CoR region. 
(Middle) A fourth contact reduces the size of the feasible twist cone. (Right) 
By changing the angle of the fourth contact normal, no twist is feasible; the 
part is in form closure. 


12.1.7.1 Number of Contacts Needed for First-Order Form Closure 

Each stationary contact i provides a half-space twist constraint of the form 

H v > o. 

Form closure holds if the only twist V satisfying the constraints is the zero twist. 
For j contacts, this condition is equivalent to 

pos({J r i,..., JFj}) = R 6 

for parts in three dimensions. Therefore, by Fact 2 from the beginning of the 
chapter, at least 6 + 1 = 7 contacts are needed for first-order form closure of 
spatial parts. For planar parts, the condition is 

pos({J r i, ...,+)})= K 3 , 

and 3 + 1 = 4 contacts are needed for first-order form closure. These results are 
summarized in the following theorem. 

Theorem 12.1. For a planar part, at least four point contacts are needed for 
first-order form closure. For a spatial part, at least seven point contacts are 
needed. 

Now consider the problem of grasping a circular disk in the plane. It should 
be clear that kinematically preventing motion of the disk is impossible regardless 
of the number of contacts; it will always be able to spin about its center. Such 
objects are called exceptional -the positive span of the contact normal forces 
at all points on the object is not equal to M n , where n = 3 in the planar case 
and n = 6 in the spatial case. Examples of such objects in three dimensions 
include surfaces of revolution, such as spheres and ellipsoids. 

Figure 12.14 shows example planar grasps. The graphical methods of Sec¬ 
tion 12.1.6 indicate that the four contacts in Figure 12.14(a) immobilize the 
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Figure 12.14: (a) Four fingers yielding planar form closure. The first-order 
analysis treats (b) and (c) identically, saying the triangle can rotate about its 
center in each case. A second-order analysis shows this is not possible for (b). 
The grasps in (d), (e), and (f) are identical by a first-order analysis, which says 
that rotation about any center on the vertical line is possible. This is true for 
(d), while only some of these centers are possible for (e). No motion is possible 
in (f). 


object. Our first-order analysis indicates that the parts in Figures 12.14(b) and 
12.14(c) can each rotate about their centers in the three-finger grasps, but in 
fact this is not possible for the part in Figure 12.14(b)—a second-order analysis 
would tell us that this part is actually immobilized. Finally, the first-order anal¬ 
ysis tells us that the two-fingered grasps in Figures 12.14(d)-(f) are identical, 
but in fact the part in Figure 12.14(f) is immobilized by only two fingers due to 
curvature effects. 

To summarize, our first-order analysis always correctly labels breaking and 
penetrating motions, but second- and higher-order effects may change first- 
order roll-slide motions to breaking or penetrating. If an object is in form 
closure by the first-order analysis, it is in form closure for any analysis; if only 
roll-slide motions are feasible by the first-order analysis, the object could be in 
form closure by a higher-order analysis; and otherwise the object is not in form 
closure by any analysis. 

12.1.7.2 A Linear Programming Test for First-Order Form Closure 

Let F = [ T\ | Ti |... | Tj ] S M nX: ' be a matrix whose columns are formed by 
the j contact wrenches. For spatial parts, n = 6, and for planar parts, n = 3 
with Ti = {rrii Z , fi x , fi y ) T . The contacts yield form closure if there exists a 
vector of weights k € , k > 0 such that Fk + F ex t = 0 for all F ex t 6 M". 

Clearly the part is not in form closure if the rank of F is not full (rank(F) < 
n). If F is full rank, the form-closure condition is equivalent to the existence of 
strictly positive coefficients k > 0 such that Fk = 0. We can formulate this test 
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Figure 12.15: Two fingers grasping the interior of an object. 


as the following linear program: 

find k (12.14) 

minimizing 1 T k 
such that Fk = 0 

ki> 1, i = l,...,j, 

where 1 is a j-vector of ones. If F is full rank and there exists a solution k to 
(12.14), the part is in first-order form closure. Otherwise it is not. Note that 
the objective function l 7 k is not necessary to answer the binary question, but it 
is included to make sure the problem is well posed, depending on the LP solver. 


Example 12.5. The planar object in Figure 12.15 has a hole in the center. 
Two fingers each touch two different edges of the hole, creating four contact 
normals. The matrix F = [ T\ | Ti \ F 3 | ] is 


F = 


0 0-12 
-10 10 

0-1 0 1 


The matrix F is clearly rank 3. The linear program of (12.14) returns a solution 
with k\ = ks = 2, &2 = k± = 1, so the grasp is form closure. If the circular 
finger were moved to the bottom right corner of the hole, the new F matrix 


F = 


0 0 0 -2 
-10 10 
0 - 10-1 


is still full rank, but there is no solution to the linear program. This grasp is 
not form closure. 


12.1.7.3 Measuring the Quality of a Form-Closure Grasp 

Consider the two form-closure grasps shown in Figure 12.16. Which is a better 
grasp? 
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Figure 12.16: Both grasps are form closure, but which is better? 

Answering this question requires a metric measuring the quality of a grasp. 
A grasp metric takes the set of contacts {A,;} and returns a single value 
Qual({Ai}), where Qual({Jq}) < 0 indicates that the grasp is not form clo¬ 
sure, and larger positive values indicate better grasps. 

There are many reasonable choices of grasp metric. As an example, suppose 
that to avoid damaging the object, we require the magnitude of the force at 
contact i be less than or equal to jy ma x > 0, which may be set to 1. Then the 
total set of contact wrenches that can be applied by the j contacts is given by 



(12.15) 


See Figure 12.17 for an example in two dimensions. This is the convex set of 
wrenches that the contacts can apply to resist disturbance wrenches applied to 
the part. If the grasp is form closure, the set includes the origin of the wrench 
space in its interior. 

Now the problem is to turn this polytope into a single number representing 
the quality of the grasp. Ideally this process would use some idea of the distur¬ 
bance wrenches the part can be expected to experience. A simpler choice is to 
set Qual({J)}) to be the radius of the largest ball of wrenches, centered at the 
origin of the wrench space, that fits inside the convex polytope. In evaluating 
this radius, two caveats should be considered: (1) moments and forces have 
different units, so there is no obvious way to equate force and moment magni¬ 
tudes, and (2) the moments due to contact forces depend on the choice of the 
location of the origin of the space frame. To address (1), it is common to choose 
a characteristic length r of the grasped part and convert contact moments m 
to forces m/r. To address (2), the origin can be chosen somewhere near the 
geometric center of the part, or at its center of mass. 

Given the choice of the space frame and the characteristic length r, we 
simply calculate the signed distance from the origin of the wrench space to 
each hyperplane on the boundary of CF. The minimum of these distances is 
Qual({A}) (Figure 12.17). 

Returning to our original example in Figure 12.16, we can see that if each 
finger is allowed to apply the same force, then the grasp on the left may be 
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Figure 12.17: (a) A set of three contact wrenches in a two-dimensional wrench 
space, and the radius d of the largest ball of wrenches centered at the origin that 
fits inside the wrench polygon, (b) A different set of three wrenches yielding a 
larger inscribed ball. 


considered the better grasp, as the contacts can resist greater moments about 
the center of the object. 

12.1.7.4 Choosing Contacts for Form Closure 

Many methods have been suggested for choosing form-closure contacts for fix- 
turing or grasping. One approach is to sample candidate grasp points on the 
surface of the object (four for planar parts or seven for spatial parts) until a 
set is found yielding form closure. From there, the candidate grasp points may 
be incrementally repositioned according to gradient ascent using the grasp met¬ 
ric, i.e., dQual (p)/dp, where p is a vector of all the coordinates of the contact 
locations. 4 


12.2 Contact Forces and Friction 

12.2.1 Friction 

A commonly used model of friction in robotic manipulation is Coulomb fric¬ 
tion. This experimental law states that the tangential friction force magnitude 
ft is related to the normal force magnitude f n by f t < pf ni where p is called the 
friction coefficient. If the contact is sliding, or currently rolling but with in¬ 
cipient slip (i.e., at the next instant the contacts are sliding), then / t = pf n , and 

4 The gradient vector d Qual (p) /dp must be projected to the tangent planes at the points 
of contact to keep the contact locations on the surface of the object. 
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Figure 12.18: (a) A friction cone illustrating all possible forces that can be 
transmitted through the contact, (b) A side view of the same friction cone 
showing the friction coefficient /r and the friction angle a = tan -1 /. i. (c) An 
inscribed polyhedral convex cone approximation to the circular friction cone. 


the direction of the friction force is opposite of the sliding direction, i.e., friction 
dissipates energy. The friction force is independent of the speed of sliding. 

Often two friction coefficients are defined, a static friction coefficient /x s and 
a kinetic (or sliding) friction coefficient /i^, where fx s > fx^. This implies that 
a larger friction force is available to resist initial motion, but once motion has 
begun, the resisting force is smaller. Many other friction models have been 
developed with different functional dependencies on factors such as the speed 
of sliding and the duration of static contact before sliding. All of these are 
aggregate models of complex microscopic behavior. For simplicity, we use the 
simplest Coulomb friction model with a single friction coefficient [i. This model 
is reasonable for hard, dry materials. The friction coefficient depends on the 
two materials in contact, and typically ranges from 0.1 to 1. 

For a contact normal in the +z direction, the set of forces that can be 
transmitted through the contact satisfies 

sjfl + 51 < M/*, fz > 0. (12.16) 

Figure 12.18(a) shows that this set of forces forms a friction cone. The set 
of forces that the finger can apply to the plane lies inside the cone shown. 
Figure 12.18(b) shows the same cone from a side view, illustrating the friction 
angle a = tan -1 fx, which is the half-angle of the cone. If the contact is not 
sliding, the force may be anywhere inside the cone. If the finger slides to the 
right, the force it applies lies on the right edge of the friction cone, with a 
magnitude determined by the normal force. Correspondingly, the plane applies 
the opposing force to the finger, and the tangential (frictional) portion of this 
force opposes the sliding direction. 

To allow linear formulations of contact mechanics problems, it is often con¬ 
venient to represent the convex circular cone by a polyhedral convex cone. Fig- 
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ure 12.18(c) shows an inscribed four-sided pyramidal approximation of the fric¬ 
tion cone, defined by the positive span of the (fx>fy,fz) cone edges (p,0, 1), 
(— p, 0,1), (0,/r, 1), and (0, —p,, 1). We can obtain a tighter approximation to 
the circular cone by using more edges. An inscribed cone underestimates the 
friction forces available, while a circumscribed cone overestimates the friction 
forces. The choice of which to use depends on the application. For example, if 
we want to ensure that a robot hand can grasp an object, it is a good idea to 
underestimate the friction forces available. 

For planar problems, no approximation is necessary—a friction cone is ex¬ 
actly represented by the positive span of the two edges of the cone, similar to 
the side view illustrated in Figure 12.18(b). 

Once we choose a coordinate frame, any contact force can be expressed as 
a wrench T = {[p]f, /), where p is the contact location. This turns the friction 
cone into a wrench cone. A planar example is shown in Figure 12.19. The 
two edges of the planar friction cone give two rays in the wrench space, and the 
wrenches that can be transmitted to the part through the contact is the positive 
span of basis vectors along these edges. If T\ and Ti are basis vectors for these 
wrench cone edges, we write the wrench cone as WC = pos({Ji, Fi}). 

If multiple contacts act on a part, then the total set of wrenches that can 
be transmitted to the part through the contacts is the positive span of all the 
individual wrench cones WC;, 




WC = pos({WC i }) = \ ^ | Ti G WCi,ki > 0 


This composite wrench cone is a convex cone rooted at the origin. An example 
composite wrench cone is shown in Figure 12.19(d) for a planar object with the 
two friction cones shown in Figure 12.19(c). For planar problems, the composite 
wrench cone in the three-dimensional wrench space is polyhedral. For spatial 
problems, wrench cones in the six-dimensional wrench space are not polyhedral, 
unless the individual friction cones are approximated by polyhedral cones, as in 
Figure 12.18(c). 

If a contact or set of contacts acting on a part is ideally force-controlled, the 
wrench J^ont specified by the controller must lie within the composite wrench 
cone corresponding to those contacts. If there are other non-force-controlled 
contacts acting on the part, then the cone of possible wrenches on the part 
is equivalent to the wrench cone from the non-force-controlled contacts, but 
translated to be rooted at J r C ont- 

12.2.2 Planar Graphical Methods 

12.2.2.1 Representing Wrenches 

Any planar wrench T = (m z , f x , f y ) with a nonzero linear component can be 
represented as an arrow drawn in the plane, where the base of the arrow is at 
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Figure 12.19: (a) A planar friction cone with friction coefficient y and corre¬ 
sponding friction angle a = tan -1 ^. (b) The corresponding wrench cone, (c) 
Two friction cones, (d) The corresponding composite wrench cone. 


the point 

(x,y) = 2 2 {m z f y , —m z f x ), 

J x ' J y 

and the head of the arrow is at [x + f x ,y + f y ). The moment is unchanged if we 
slide the arrow anywhere along the line of the arrow, so any arrow of the same 
direction and length along the line represents the same wrench (Figure 12.20). 
If fx = fy = 0 and m z ^ 0, the wrench is a pure moment, and we do not try to 
represent it graphically. 

Two wrenches, represented as arrows, can be summed graphically by sliding 
the arrows along their lines until the bases of the arrows are coincident. The 
arrow corresponding to the sum of the two wrenches is obtained as shown in Fig¬ 
ure 12.20. The approach can be applied sequentially to sum multiple wrenches 
represented as arrows. 
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Figure 12.20: (Left) The planar wrench F = (m z , f x , f y ) = (2.5,—1,2) repre¬ 
sented as an arrow in the x-y plane. (Middle) The same wrench can be repre¬ 
sented by an arrow anywhere along the line of action. (Right) Two wrenches 
are summed by sliding their arrows along their lines of action until the bases 
of the arrows are coincident, then doing a vector sum by the parallelogram 
construction. 


12.2.2.2 Representing Wrench Cones 

In the previous section, each wrench had a specified magnitude. But a rigid- 
body contact implies that the contact normal force can be unbounded; it is 
whatever is needed to prevent two bodies from penetrating. Therefore it is 
useful to have a representation of all wrenches of the form kF, where k > 0 and 
T G R 3 is a basis vector. 

One such representation is moment labeling. The arrow for the basis 
wrench F is drawn as in Section 12.2.2.1. Then all points in the plane to the 
left of the line of the arrow are labeled indicating that any positive scaling of 
F creates a positive moment m z about those points, and all points in the plane 
to the right of the arrow are labeled *—,’ indicating that any positive scaling of 
F creates a negative moment about those points. Points on the line are labeled 
! ±. 

Generalizing, moment labels can represent any homogeneous convex pla¬ 
nar wrench cone, much like a homogeneous convex planar twist cone can be 
represented as a convex CoR region. Given a collection of directed force lines 
corresponding to wrenches kjFi for all ki > 0, the wrench cone posd-F,}) can 
be represented by labeling each point in the plane with a ‘+’ if each of the F t 
makes nonnegative moment about that point, a ’ if each F% makes nonpositive 
moment about that point, a ‘± ! if each makes zero moment about that point, 
or a blank label if at least one wrench makes positive moment and at least one 
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Figure 12.21: (a) Representing a line of force by moment labels, (b) Represent¬ 
ing the positive span of two lines of force by moment labels, (c) The positive 
span of three lines of force. 


wrench makes negative moment about that point. 

The idea is best illustrated by an example. In Figure 12.21(a), the basis 
wrench T\ is represented by labeling the points to the left of the line with a + 
and points to the right of the line with a —. Points on the line are labeled ±. In 
Figure 12.21(b), another basis wrench is added, which could represent the other 
edge of a planar friction cone. Only the points in the plane that are consistently 
labeled for both lines of force retain their labels; inconsistently labeled points 
lose their labels. Finally, a third basis wrench is added in Figure 12.21(c). The 
result is a single region labeled +. A nonnegative linear combination of the 
three basis wrenches can create any line of force in the plane that passes around 
this region in a counterclockwise sense. No other wrench can be created. 

If an additional basis wrench were added passing clockwise around the region 
labeled + in Figure 12.21(c), then there would be no consistently labeled point 
in the plane: the positive linear span of the four wrenches is the entire wrench 
space R 3 . 

The moment-labeling representation is equivalent to a homogeneous con¬ 
vex wrench cone representation. The moment labeling regions in each of Fig¬ 
ure 12.21(a), (b), and (c) is properly interpreted as a single convex region, much 
like the CoR regions of Section 12.1.6. 

12.2.3 Force Closure 

Consider a single movable object and a number of frictional contacts. We say 
the contacts result in force closure if the composite wrench cone contains the 
entire wrench space—any external wrench lF ex t on the object can be balanced 
by contact forces. 

We can derive a simple linear test for force closure which is exact for planar 
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Figure 12.22: An equilateral triangle can be force-closure grasped by two fingers 
on edges of the triangle if p > tan 30° « 0.577. (a) This grasp with p = 0.25 
is not force closure, as indicated by the consistently labeled moment-labeling 
region, (b) This grasp is force closure with p = 1. 


cases and approximate for spatial cases. Let Fi,i = 1 ■ ■ ■ j ■ be the wrenches 
corresponding to the edges of the friction cones for all the contacts. For planar 
problems, each friction cone contributes two edges, and for spatial problems, 
each friction cone contributes three or more edges, depending on the polyhedral 
approximation chosen (see Figure 12.18(c)). The columns of the n x j matrix 
F are the J-), where n = 3 for planar problems and n = 6 for spatial problems. 
Now the test for force closure is identical to that for form closure. The contacts 
yield force closure if 

• rank(F') = n, and 

• there exists a solution to the linear programming problem (12.14). 

In the case of p = 0, each contact can provide forces only along the normal 
direction, and force closure is equivalent to first-order form closure. 

12.2.3.1 Number of Contacts Needed for Force Closure 

For planar problems, four contact wrenches are sufficient to positively span 
the three-dimensional wrench space, which means that as few as two frictional 
contacts (with two friction cone edges each) are sufficient for force closure. Using 
moment labeling, we see that force closure is equivalent to having no consistent 
moment labels. For example, if the two contacts can “see” each other by a line 
inside both friction cones, we have force closure (Figure 12.22). 

It is important to note that force closure simply means that the contact 
friction cones can generate any wrench. It does not necessarily mean that the 
object will not move in the presence of an external wrench, however. For the 
example of Figure 12.22(b), whether the triangle falls under gravity or not 
depends on the internal forces between the fingers. If the motors powering the 
fingers cannot provide sufficient forces, or if they are restricted to only generate 
forces in certain directions, the triangle may fall despite force closure. 
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Figure 12.23: A spatial rigid body restrained by three point contacts with fric¬ 
tion. 



Figure 12.24: Three possibilities for the intersection between a friction cone and 
a plane. 


Two frictional point contacts are insufficient to yield force closure for spatial 
parts, as there is no way to generate moment about the axis joining the two 
contacts. A force-closure grasp can be obtained with as few as three frictional 
contacts, however. A particularly simple and appealing result due to Li et al. [65] 
reduces the force closure analysis of spatial frictional grasps to a planar force 
closure problem. Referring to Figure 12.23, suppose a rigid body is constrained 
by three point contacts with friction. If the three contact points happen to be 
collinear, then obviously any moment applied about this line cannot be resisted 
by the three contacts. We can therefore exclude this case, and assume that the 
three contact points are not collinear. The three contacts then define a unique 
plane S , and at each contact point, three possibilities arise (see Figure 12.24): 

• The friction cone intersects S' in a planar cone; 

• The friction cone intersects S in a line; 

• The friction cone intersects S in a point. 

The object is in force closure if and only if each of the friction cones intersects 
S in a planar cone, and S is also in planar force closure. 

Theorem 12.2. Given a spatial rigid body restrained by three point contacts 
with friction, the body is in force closure if and only if the friction cone at each 
of the contacts intersects the plane S of the contacts in a cone, and the plane S 
is in planar force closure. 
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Proof. First, the necessity condition—if the spatial rigid body is in force closure, 
then each of the friction cones intersects S in a planar cone and S is also in planar 
force closure—is easily verified: if the body is in spatial force closure, then S 
(which is a part of the body) must also be in planar force closure. Moreover, if 
even one friction cone intersects S in a line or point, then there will be external 
moments (e.g., about the line between the remaining two contact points) that 
cannot be resisted by the grasp. 

To prove the sufficiency condition if each of the friction cones intersects S 
in a planar cone and S is also in planar force closure, then the spatial rigid body 
is in force closure—choose a fixed reference frame such that S lies in the x-y 
plane, and let r, € R 3 denote the vector from the fixed frame origin to contact 
point i (see Figure 12.23). Denoting the contact force at i by fi £ R 3 , the 
contact wrench T. £ R 6 is then of the form 


Ti = 


rrii 

fi 


(12.17) 


where each Wj 
T ext S R 6 by 


ri x fi, i = 1,2,3. Denote the arbitrary external wrench 


Te, 


^ext 

/ext 


€ R 6 . 


(12.18) 


Force closure then requires that there exist contact wrenches T x . i = 1, 2, 3, each 
lying inside its respective friction cone, such that for any external disturbance 
wrench T ex t, the following equality is satisfied: 


or equivalently, 


Ti+T 2 +T 3 +T e xt — 0 , 

fl + /2 + / 3 + /ext 
(d x ft) + (r 2 X / 2 ) + (r 3 X / 3 ) + TOext 


0 

0 , 


(12.19) 


( 12 . 20 ) 

( 12 . 21 ) 


If each of the contact forces and moments, as well as the external force and mo¬ 
ment, is orthogonally decomposed into components lying on the plane spanned 
by S (corresponding to the x-y plane in our chosen reference frame) and its 
normal subspace N (corresponding to the 2 -axis in our chosen reference frame), 
then the previous force closure equality relations can be written 


/is + / 2 s + / 3 s = 

/ext, S 

(12.22) 

(D X f is ) + (r 2 x f 2S ) + (r 3 x f 3S ) = 

^ext ,5 

(12.23) 

f\N + $2N + fsN = 

/ext,iV 

(12.24) 

{n x f 1N ) + (r 2 x f 2N ) + (r 3 x f 3N ) = 

^ext,iV • 

(12.25) 


In what follows we shall use S to refer both to the slice of the rigid body 
corresponding to the x-y plane, as well as the x-y plane itself. N will always be 
identified with the z-axis. 
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Proceeding with the proof of sufficiency, we now show that if S is in pla¬ 
nar force closure, then the body is in spatial force closure. In terms of Equa¬ 
tions (12.24)-(12.25) we wish to show that, for any arbitrary forces f ex t,s £ S, 
/ext, n £ N and arbitrary moments m ex t,s £ S, m ex t,Ar £ N, there exist contact 
forces fis £ S, fiN € N, i = 1,2,3, that satisfy (12.24)-(12.25), and such that 
for each i = 1,2, 3, the contact force /,; = fag + fay lies in friction cone i. 

First consider the force closure equations (12.24)-(12.25) in the normal di¬ 
rection N. Given an arbitrary external force f ex t,N £ N and external moment 
m ex t,s £ S, Equations (12.24)-(12.25) constitute a set of three linear equations 
in three unknowns. From our assumption that the three contact points are never 
collinear, these equations will always have a unique solution set { f * N , f£ N , / 3Ar } 
in N. 

Since S is assumed to be in planar force closure, for any arbitrary f ex t,s £ S 
and m ex t,N £ -IV, there will exist planar contact forces fig € S, i = 1,2,3, that 
lie inside their respective planar friction cones and also satisfy Equations (12.22)- 
(12.23). This solution set is not unique: one can always find a set of internal 
forces rji £ S, i = 1,2,3, each lying inside its respective friction cone, satisfying 

V 1 +V 2 +V 3 = 0 (12.26) 

(ri x rji) + (r 2 x r/ 2 ) + (r 3 x ry 3 ) = 0. (12.27) 

(To see why such r)i exist, recall that since S is assumed to be in planar force 
closure, solutions to (12.22)-(12.23) must exist for f ex t,s = ^ext ,n = 0; these 
solutions are precisely the internal forces rji). Note that these two equations 
constitute three linear equality constraints involving six variables, so that there 
exists a three-dimensional linear subspace of solutions for {r?i, %}■ 

Now if {/is,/ 2 s,/ 3 s} satisfy (12.22)-(12.23), then so will {/is + m,hs + 
^? 2 , / 3 s + 773 }. The internal forces { 771 , 772 , ^ 3 } can in turn be chosen to have 
sufficiently large magnitude so that the contact forces 


h = fiN + fis + m (12.28) 

fl = /2AT + /2S+ 7 ?2 (12.29) 

fs = / 3 W + / 3 s + V3 (12.30) 

all lie inside their respective friction cone. This completes the proof of the 
sufficiency condition. □ 


12.2.3.2 Measuring the Quality of a Force-Closure Grasp 

Friction forces are not always repeatable. For example, try putting a coin on a 
book and tilting the book. The coin should begin to slide when the book is at an 
angle a = tan -1 /i with respect to horizontal. If you do the experiment several 
times, you may find a range of measured values of /x, however, due to effects 
that are difficult to model. For that reason, when choosing between grasps, 
it is reasonable to choose finger locations that minimize the friction coefficient 
needed to achieve force closure. 


428 


Grasping and Manipulation 


12.2.4 Duality of Force and Motion Freedoms 

Our discussion of kinematic constraints and friction should make apparent that, 
for any point contact and contact label, the number of equality constraints on 
the part’s motion caused by that contact is equal to the number of wrench 
freedoms it provides. For example, a breaking contact B provides zero equality 
constraints on the part motion and also allows no contact force. A fixed contact 
R provides 3 motion constraints (the motion of a point on the part is specified) 
and 3 freedoms on the contact force: any wrench in the interior of the contact 
wrench cone is consistent with the contact mode. Finally, a slipping contact 
S provides 1 equality motion constraint (one equation on the part’s motion 
must be satisfied to maintain the contact), and for a given motion satisfying the 
constraint, the contact wrench has only 1 freedom, the magnitude of the contact 
wrench on the edge of the friction cone and opposing the slipping direction. In 
the planar case, the motion constraints and wrench freedoms for B, S, and R 
contacts are 0, 1, and 2, respectively. 

12.3 Manipulation 

So far we have studied the feasible twists and contact forces due to a set of 
contacts. We have also considered two types of manipulation: form- and force- 
closure grasping. 

Manipulation consists of much more than just grasping, however. It includes 
almost anything where manipulators impose motions or forces with the purpose 
of achieving motion or restraint of objects. Examples include carrying glasses 
on a tray without toppling them, pivoting a refrigerator about a foot, pushing a 
sofa on the floor, throwing and catching a ball, transporting parts on a vibratory 
conveyor, etc. Endowing a robot with methods of manipulation beyond grasp- 
and-carry allows it to manipulate several parts simultaneously; manipulate parts 
that are too large to be grasped or too heavy to be lifted; or even to send parts 
outside the workspace of the end-effector by throwing. 

To plan such manipulation tasks, we use the contact kinematic constraints 
of Section 12.1, the Coulomb friction law of Section 12.2, and the dynamics of 
rigid bodies. Restricting ourselves to a single rigid body and using the notation 
of Chapter 8, the part’s dynamics are written 

ext + ]T UTi = QV - [ad v ] T gV, h > 0, 7)6 WC U (12.31) 

where V is the part’s twist, Q is its spatial inertia matrix, T eyA is the external 
wrench acting on the part due to gravity, etc., WCi is the set of possible wrenches 
acting on the object due to contact i, and ]T) is the wrench due to the 
contacts, and all wrenches are written in the part’s center of mass frame. Now, 
given a set of motion- or force-controlled contacts acting on the part, and the 
initial state of the system, one method for solving for the motion of the part is 
the following: 
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(i) Enumerate the set of possible contact modes considering the current state 
of the system (e.g., a contact that is currently sticking can transition to 
sliding or breaking). The contact modes consist of the contact labels R, S, 
and B at each contact. 

(ii) For each contact mode, determine if there exists a contact wrench ^ ki.Fj, 
that is consistent with the contact mode and Coulomb’s law, and an accel¬ 
eration V consistent with the kinematic constraints of the contact mode, 
such that Equation (12.31) is satisfied. If so, this contact mode, contact 
wrench, and part acceleration is a consistent solution to the rigid-body 
dynamics. 

This kind of “case analysis” may sound unusual; we are not simply solving 
a set of equations. It also leaves open the possibility that we could find more 
than one consistent solution, or perhaps no consistent solution. This is, in 
fact, the case: we can define problems with multiple solutions (ambiguous) 
and problems with no solutions (inconsistent). This state of affairs is a bit 
unsettling; surely there is exactly one solution to any real mechanics problem! 
But this is the price we pay to use the assumptions of perfectly rigid bodies and 
Coulomb friction. Also, for many problems the method described above will 
yield a unique contact mode and motion. 

In this chapter we do not provide tools to solve the general simulation prob¬ 
lem. Instead, we focus on manipulation problems that permit solutions using our 
first-order kinematic analysis. We also consider quasistatic problems, where 
the velocities and accelerations of the parts are small so that inertial forces 
may be ignored. Contact wrenches and external wrenches are always in force 
balance, and Equation (12.31) reduces to 

F 3xt + ^2 hFi = 0, hi > 0, Ti e WCi. (12.32) 

Below we illustrate the methods of this chapter with several examples. 

Example 12.6. A block carried by two fingers. 

Consider a planar block in gravity supported by two fingers, as in Figure 12.25(a). 
The friction coefficient between one finger and the block is p = 1, and the other 
contact is frictionless. Thus the cone of wrenches that can be applied by the 
fingers is pos({J r i, .F 3 }), as shown using moment labeling in Figure 12.25(b). 

Our first question is whether the stationary fingers can keep the block at 
rest. To do so, the fingers must provide a wrench F = (m 2 , f x , f y ) = (0,0, mg) 
to balance J^xt = (0, 0, —mg) due to gravity, where g > 0. As shown in Fig¬ 
ure 12.25(b), however, this wrench is not in the composite cone of possible 
contact wrenches. Therefore the contact mode RR is not feasible, and the block 
will move relative to the fingers. 

Now consider the case where the fingers each accelerate to the left at 2 g. In 
this case, the contact mode RR requires that the block also accelerate to the left 
at 2 g. The wrench needed to cause this acceleration is (0, —2mg, 0). Therefore 
the total wrench that the fingers must apply to the block is ( 0 , —2mg, 0)—F ext = 
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Figure 12.25: (a) A planar block in gravity supported by two robot fingers, one 
with a friction cone with p = 1 and one with p = 0. (b) The composite wrench 
cone that can be applied by the fingers represented using moment labels. To 
balance the block against gravity, the fingers must apply the line of force shown. 
This line makes positive moment with respect to some points labeled —, and 
therefore it cannot be generated by the two fingers, (c) For the block to match 
the fingers’ acceleration to the left, the contacts must apply the vector sum of 
the wrench to balance gravity plus the wrench needed to accelerate the block 
to the left. This total wrench lies inside the composite wrench cone, as the 
line of force makes positive moment with respect to the points labeled + and 
negative moment with respect to the points labeled —. (d) The total wrench 
applied by the fingers in Figure (c) can be translated along the line of action 
without changing the wrench. This allows us to easily visualize the components 
fci.Fi + fc’ 2 F 2 and provided by the fingers. 


(0, —2m g, mg). As shown in Figures 12.25(c) and (d), this wrench lies inside the 
composite wrench cone. Thus RR (the block stays stationary relative to the 
fingers) is a solution as the fingers accelerate to the left at 2 g. 

This is called a dynamic grasp —inertial forces are used to keep the block 
pressed against the fingers while the fingers move. If we plan to manipulate the 
block using a dynamic grasp, we should make certain that no contact modes 
other than RR are feasible, for completeness. 

Moment labels are convenient for understanding this problem graphically, 
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but we can also solve it algebraically. Finger one contacts the block at (x, y ) = 
(—3, —1) and finger 2 contacts the block at (1,1). This gives the basis contact 
wrenches 

■F„ = (1,-1,0) T . 

Let the fingers’ acceleration in the x direction be written a x . Then, under 
the assumption that the block stays fixed to the fingers (RR contact mode), 
Equation (12.31) can be written 


h+ k 2 T 2 + k 3 J 3 + (0,0, —mg) = (0, m a x , 0). (12.33) 


This yields three equations in the three unknowns, k±,k 2 , k 3 . Solving, we get 

fci =-^=(a x +g)m, k 2 =-^=(a x + 5g)m, k 3 = -^(a x - 3g)m. 

For the fc; to be nonnegative, we need —5 g < a x < —g. For x-direction finger 
accelerations in this range, a dynamic grasp is a consistent solution. 


Example 12.7. The meter stick trick. 

Try this experiment: Get a meter stick (or any similar long smooth stick) and 
balance it horizontally on your two index fingers. Place your left finger near 
the 10 cm mark and your right finger near the 60 cm mark. The center of mass 
is closer to your right finger, but still between your fingers, so that the stick 
is supported. Now, keeping your left finger stationary, slowly move your right 
finger towards your left until they touch. What happens to the stick? 

If you didn’t try the experiment, you might guess that your right finger 
passes under the center of mass of the stick, at which point the stick falls. If 
you did try the experiment, you saw something different. Let’s see why. 

Figure 12.26 shows the stick supported by two frictional hngers. Since all 
motions are slow, we use the quasistatic approximation that the stick’s accelera¬ 
tion is zero, and the net contact wrench must balance the gravitational wrench. 
As the two fingers move together, the stick must slip on one or both fingers to 
accommodate the fact that the fingers are getting closer to each other. Fig¬ 
ure 12.26 shows the moment-labeling representation of the composite wrench 
cone for three different contact modes where the stick remains stationary on the 
left finger (R) or slips left relative to it (Si) while either remaining stationary 
on the right finger (R) or slipping right relative to it (Sr). It is clear from the 
figure that only the SIR contact mode can provide a wrench that balances the 
gravitational wrench. In other words, the right finger, which supports more of 
the stick’s weight, remains sticking, while the left finger slides under the stick. 
Since the right finger is moving to the left in the world frame, this means the 
center of mass is moving to the left at the same speed. This continues until the 
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Figure 12.26: Top left: Two frictional fingers supporting a meter stick in gravity. 
The other three panels show the moment labels for the RSr, SIR, and SISr 
contact modes. Only the SIR contact mode yields force balance. 


center of mass is halfway between the fingers, at which point the stick transi¬ 
tions to the SISr contact mode, and the center of mass stays centered between 
the fingers until they meet. The stick never falls. 

Note that this analysis relies on the quasistatic assumption. It is easy to 
make the stick fall if you move your right finger quickly; the friction force at 
the right finger is not large enough to create the large stick acceleration needed 
to maintain a sticking contact. Also, in your experiment, you might notice that 
when the center of mass is nearly centered, the stick does not actually achieve 
the idealized SISr contact mode, but instead switches rapidly between the SIR 
and RSr contact modes. This is because the static friction coefficient is larger 
than the kinetic friction coefficient. 

Example 12.8. Stability of an assembly. 
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16 


Figure 12.27: Left: An arch in gravity. Right: The friction cones at the contacts 
of the stones 1 and 2. 

Consider the arch in Figure 12.27. Is it stable under gravity? 

For a problem like this, graphical planar methods are difficult to use, since 
there are potentially multiple moving parts. Instead we test algebraically for 
consistency of the contact mode with all contacts labeled R. The friction cones 
are shown in Figure 12.27. With these labelings of the friction cone edges, 
the arch remaining standing is a consistent solution if there exist ki > 0 for 
i = 1... 16 satisfying the following nine wrench-balance equations, three for 
each body: 


8 


^ ^ ki,J~i T -Text 1 0 



*=9 
12 


^ ' kiJ~i T *Fext3 0. 


The last set of equations comes from the fact that wrenches that body 1 applies 
to body 3 are equal and opposite those that body 3 applies to body 1, and 
similarly for bodies 2 and 3. 

This linear constraint satisfaction problem can be solved by a variety of 
methods, including linear programming. 

Example 12.9. Peg insertion. 

Figure 12.28 shows a force-controlled planar peg in two-point contact with a 
hole during insertion. Also shown are the contact friction cones acting on the 
peg and the corresponding composite wrench cone, illustrated using moment 
labels. If the force controller applies the wrench T\ to the peg, it may jam 
the hole may generate contact forces that balance T\. Therefore the peg may 
get stuck in this position. If the force controller applies the wrench J~ 2 , however, 
the contacts cannot balance the wrench and insertion proceeds. 

If the friction coefficients at the two contacts are large enough that the two 
friction cones “see” each others’ base, the peg is in force closure, and the contacts 
may be able to resist any wrench (depending on the internal force between the 
two contacts). The peg is said to be wedged. 
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Figure 12.28: Left: A peg in two-point contact with a hole. Right: The wrench 
J ~i may cause the peg to jam, while the wrench T-i continues to push the peg 
into the hole. 

12.4 Summary 

• Three ingredients are needed to solve rigid-body contact problems with 
friction: (1) contact kinematics, which describe the feasible motions of 
rigid bodies in contact; (2) a contact force model, which describes what 
forces can be transmitted through frictional contacts; and (3) rigid-body 
dynamics, as described in Chapter 8. 

• Let two rigid bodies, A and B, be in point contact at pa in a space frame. 
Let n £ R 3 be the unit contact normal, pointing into body A. Then the 
spatial contact wrench J- associated with a unit force along the contact 
normal is T = (([ pA]n) T n T ) T . The impenetrability constraint is 

T t (V a - V B ) > 0, 

where Va and Vb are the spatial twists of A and B. 

• A contact that is sticking or rolling is assigned the contact label R, a con¬ 
tact that is sliding is assigned the contact label S, and a contact that is 
breaking free is given the contact label B. For a body with multiple con¬ 
tacts, the contact mode is the concatenation of the labels of the individual 
contacts. 

• A single rigid body subjected to multiple stationary point contacts has a 
homogeneous (rooted at the origin) polyhedral convex cone of twists that 
satisfy all the impenetrability constraints. 

• A homogeneous polyhedral convex cone of planar twists in R 3 can be 
equivalently represented by a convex region of signed rotation centers in 
the plane. 

• If a set of stationary contacts prevents an object from moving, purely by 
a kinematic analysis considering only the contact normals, the object is 
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said to be in first-order form closure. The contact wrenches J 7 ,; for contacts 
i = 1... j positively span R”, where n = 3 for the planar case and n = 6 
for the spatial case. 

• At least four point contacts are required for first-order form closure of a 
planar part, and at least seven point contacts are required for first-order 
form closure of a spatial part. 

• The Coulomb friction law states that the tangential frictional force mag¬ 
nitude ft at a contact satisfies ft < /x/ n , where fi is the friction coefficient 
and /„ is the normal force. When the contact is sticking, the frictional 
force can be anything satisfying this constraint. When the contact is slid¬ 
ing, ft = fi /n, and the direction of the friction force opposes the direction 
of sliding. 

• Given a set of frictional contacts acting on a body, the wrenches that can 
be transmitted through these contacts is the positive span of the wrenches 
that can be transmitted through the individual contacts. These wrenches 
form a homogeneous convex cone. If the body is planar, or if the body 
is spatial but the contact friction cones are approximated by polyhedral 
cones, the wrench cone is also polyhedral. 

• A homogeneous convex cone of planar wrenches in R 3 can be represented 
as a convex region of moment labels in the plane. 

• An object is in force closure if the homogeneous convex cone of contact 
wrenches from the stationary contacts is the entire wrench space (R 3 or 
R 6 ). If the contacts are frictionless, force closure is equivalent to first-order 
form closure. 

12.5 Notes and References 

The kinematics of contact draw heavily from concepts in linear algebra (see, 
for example, the texts [131, 87]) and, more specifically, screw theory [4, 93, 13, 
1, 86]. Graphical methods for analysis of planar constraints were introduced 
by Reuleaux [104], and Mason introduced graphical construction of contact 
labels for planar kinematics and moment labels for representation of homoge¬ 
neous wrench cones [81, 82]. Polyhedral convex cones, and their application 
in representing feasible twist cones and contact wrench cones, are discussed 
in [82, 46, 30, 37]. The formalization of the friction law used in this chapter was 
given by Coulomb in 1781 [21]. Surprising consequences of Coulomb friction are 
problems of ambiguity and inconsistency [69, 82, 85] and that infinite friction 
does not necessarily prevent slipping at an active contact [75]. 

Form closure and force closure are discussed in detail in the Handbook of 
Robotics [100]. In particular, that reference uses the term “frictional form 
closure” to mean the same thing that “force closure” means in this chapter. 
According to [100], force closure additionally requires that the hand doing the 
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grasping be sufficiently capable of controlling the internal “squeezing” forces. 
Similar distinctions are made in [9] and the reviews [11, 10]. In this chapter 
we do not consider the details of the robot hand and adopt a definition of force 
closure based solely on the geometry and friction of the contacts. 

The numbers of contacts needed for planar and spatial form closure were 
established by Rouleaux [104] and Somoff [127], respectively. Other foundational 
results in form and force closure grasping are developed in [56, 89, 78] and are 
reviewed in [10, 100]. An overview of grasp quality metrics is given in [100]. 
The result that two friction cones that can “see” each other’s base are sufficient 
for planar force closure was first reported in [92], and the result reviewed in this 
chapter on three-finger force-closure grasps in 3D appeared in [65]. Salisbury 
applied Griibler’s formula to calculate the mobility of a grasped object using 
kinematic models of contact like those in Table 12.1 [84], 

Second-order models of contact constraints were introduced by Rimon and 
Burdick [106, 105, 107, 108] and used to show that curvature effects allow form 
closure by fewer contacts. 

Jamming and wedging in robotic insertion were described in [125, 91, 139], 
and the notion of a dynamic grasp was first introduced in [83]. 

An important class of methods for simulating systems of rigid bodies in 
frictional contact, not covered in this chapter, are based on solving linear and 
nonlinear complementarity problems [130, 94, 135]. These complementarity 
formulations directly encode the fact that if a contact is breaking, then no force 
is applied; if a contact is sticking, then the force can be anywhere inside the 
friction cone; and if a contact is sliding, the force is on the edge of the friction 
cone. 

General references on contact modeling and manipulation include Handbook 
of Robotics chapters [46, 100] and the texts by Mason [82] and Murray et al. [90]. 
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Figure 12.29: A triangle in contact with five stationary fingers, yielding first- 
order form closure and therefore force closure. You will analyze the contact 
when one or more of the fingers are removed. The hypotenuse of the triangle is 
45° from vertical on the page, and contact normal 5 is 22.5° from vertical. 

12.6 Exercises 


1. Prove that the impenetrability constraint (12.4) is equivalent to the con¬ 
straint (12.7). 

2. (a) Consider the two planar twists Vi = (u z i,v x \,v y i) = (1,2,0) and V 2 = 
(ui z2: v X 2, v y2 ) = (1,0, —1). Draw the corresponding CoRs in a planar coordinate 
frame, and illustrate pos({Vi,V 2 j-) as CoRs. (b) Draw the positive span of 
Vi = (u z i,Vxi,Vyi) = ( 1 , 2 , 0 ) and V 2 = ( uj z2 ,v x2 ,v y2 ) = (-1,0,-1) as CoRs. 

3. A rigid body is contacted at p = (1,2,3) with a contact normal into the 
object n = (0,1,0). Write the constraint on the body’s twist V due to this 
contact. 

4. A space frame {s} is defined at a contact between a stationary constraint and 
an object. The contact normal, into the object, is the z-axis of the {s} frame, 
(a) Write the constraint on the object’s twist V if the contact is a frictionless 
point contact, (b) Write the constraints on V if the contact is a point contact 
with friction, (c) Write the constraints on V if the contact is a soft contact. 

5. Figure 12.29 shows five stationary “fingers” contacting an object. The object 
is in first-order form closure and therefore force closure. If we take away one 
finger, the object may still be in form closure. For which subsets of four fingers 
is the object still in form closure? Prove your answers using graphical methods. 

6. Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is 
contacted only by fingers 1 and 2. Label the feasible CoRs with their contact 
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labels. 

7. Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is 
contacted only by fingers 2 and 3. Label the feasible CoRs with their contact 
labels. 

8. Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is 
contacted only by fingers 2 and 3. Label the feasible CoRs with their contact 
labels. 

9. Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is 
contacted only by fingers 1 and 5. Label the feasible CoRs with their contact 
labels. 

10. Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 
is contacted only by fingers 1, 2, and 3. 

11. Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 
is contacted only by fingers 1, 2, and 4. 

12. Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 
is contacted only by fingers 1, 3, and 5. 

13. Refer to the triangle of Figure 12.29. (a) Draw the wrench cone from 
contact 5, assuming a friction angle a = 22.5° (a friction coefficient p = 0.41), 
using moment labeling, (b) Add contact 2 to the moment-labeling drawing. 
The friction coefficient at contact 2 is p = 1. 

14. Refer to the triangle of Figure 12.29. Draw the moment-labeling region 
corresponding to contact 1 with p = 1 and contact 4 with p = 0. 

15. (a) The planar grasp of Figure 12.30 consists of five frictionless point 
contacts. The square is of size 4x4. Show that this grasp is not force closure, 
(b) The grasp of part (a) can be made force closure by adding one frictionless 
point contact. Draw all the possible locations for this contact. 

16. Assume all contacts shown in Figure 12.31 are frictionless point contacts. 
Determine whether the grasp is force closure. If it is not, how many additional 
frictionless point contacts are needed to construct a force closure grasp? 

17. (a) In Figure 12.32-(a), the planar object with two rectangular holes is re¬ 
strained from the interior by four frictionless point contacts. Determine whether 
this grasp is force closure. 
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Figure 12.30: A 4 x 4 planar square restrained by five frictionless point contacts. 



Figure 12.31: A planar disk restrained by three frictionless point contacts. 


(b) In Figure 12.32-(b), the same planar object with two rectangular holes is 
now restrained from the interior by three frictionless point contacts. Determine 
whether this grasp is force closure. 

18. The planar object of Figure 12.33 is restrained by four frictionless point 
contacts. 

(a) Determine if this grasp is force closure. 

(b) Suppose the contacts A, B, C, D are now allowed to slide along the half¬ 
circle (without crossing each other). Describe the set of all possible force closure 
grasps. 

19. (a) Determine whether the grasp of Figure 12.34-(a) is force closure. As¬ 
sume all contacts are frictionless point contacts. If the grasp is not force closure, 
slide the position of one of the contacts in order to construct a force closure 
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Figure 12.32: A planar rigid body with two square holes. 



2 


2 


Figure 12.33: A planar wedge-like object with a hole. 


grasp. 

(b) Now place two frictionless point contacts at the corners as shown in Fig¬ 
ure 12.34-(b). Determine if this grasp is force closure. 

(c) In the grasp of Figure 12.34-(c), contact A is a point contact with friction 
(its friction cone is 90° as indicated in the figure), while contacts B and C are 
frictionless point contacts. Determine whether this grasp is force closure. 

20. Determine whether the grasp of Figure 12.35 is force closure. Assume all 
contacts are point contacts with a friction coefficient p = 1. 
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(a) (b) 

2 



Figure 12.34: A planar rigid body restrained by point contacts. 


21. (a) In the planar triangle of Figure 12.36-(a), contacts A, B, and C are 
all frictionless point contacts, is this grasp force closure? If not, what type of 
external force or moment would cause the object to slip out of the grasp? 

(b) In the planar triangle of Figure 12.36-(a), suppose now that contact A is 
a point contact with friction coefficient fi = 1, while contacts B and C are 
frictionless point contacts. Determine whether the grasp shown is force closure. 

(c) Now suppose contact point A can be moved to anywhere on the hypotenuse 
of the triangle as shown in Figure 12.36-(b). Determine the range of all contact 
points A for which the grasp is force closure. 

(d) Contact points B and C are now moved as shown in Figure 12.37. Bill, 
a clever student, argues that the two contacts B and C can be replaced by a 
virtual point contact with friction (point D) with the given friction cone, and 
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1/2 


1/2 



Figure 12.35: A planar rigid body restrained by three point contacts with fric¬ 
tion. 




(b) 


Figure 12.36: A planar triangle constrained by three point contacts. 


Nguyen’s condition for force closure can now be applied to A and D, as shown 
in the right Figure 12.37. Is Bill correct? Justify your answer with a derivation 
of the appropriate force closure condition. 

22. Consider the L-shaped planar object of Figure 12.38. 

(a) Suppose both contacts are point contacts with friction coefficient p = 1. 
Determine whether this grasp is force closure. 

(b) Now suppose point contact 1 is a point contact with friction coefficient 
p = 1, while point contact 2 is frictionless. Determine whether this grasp is 
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Figure 12.37: A planar triangle constrained by three point contacts. 



Figure 12.38: An L-shaped planar object restrained by two point contacts with 
friction. 


force closure. 

(c) Suppose now that the vertical position of contact 1 is allowed to vary; denote 
its height by x. Find all positions x such that the grasp of part (b) is force 
closure. 

23 . A square is restrained by three point contacts as shown in Figure 12.39: 
contact /i is a point contact with friction coefficient /x, while contacts / 2 and 
/ 3 are frictionless point contacts. If c = | and h = find the range of values 
of n such that grasp is force closure. 

24 . In the grasp of Figure 12.40, contacts f\ and / 2 on the left are frictionless 
point contacts, while contact / 3 on the right is a point contact with friction 
coefficient /i = 0.2. Determine whether this grasp is force closure. 

25 . A single point contact with friction coefficient /x = 1 is applied to the left 
side of the square doughnut as shown in Figure 12.41. A force closure grasp can 
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Figure 12.39: A square restrained by three point contacts. 



Figure 12.40: A rectangle restrained by three point contacts. 


be constructed by adding another point contact with friction, also with p = 1. 
Draw all possible locations for this additional point contact. 

26 . (a) In the planar grasp of Figure 12.42(a), contacts A and B are frictionless, 
while contact C has friction, with friction cone as shown given by the angle /3. 
Find the smallest value of /? that makes this grasp a force closure grasp. 

(b) Now suppose contacts A and B have friction, with 90° friction cones as 
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(a) (b) 



Figure 12.42: 


Planar grasp. 


shown in Figure 12.42(b). Find the smallest value of (3 that makes this grasp a 
force closure grasp. 

27 . The square object of Figure 12.43 has four holes as shown. The four holes 
form parts of a circle. 

(a) In the planar grasp of Figure 12.43(a), contacts A, B , and C are all fric¬ 
tionless point contacts. Suppose contact B is allowed to slide along any of the 
three sides of the hole as shown in Figure 12.43(a). Find all possible locations 
for B that make this grasp force closure. 

(b) In the planar grasp of Figure 12.43(b), contacts A and B are frictionless 
point contacts, while contact C is a point contact with friction cone (defined by 
angle 0) as shown. Determine the range of angle /? so that the grasp is force 
closure. 

(c) In the planar grasp of Figure 12.43(c), contacts A and B are frictionless point 
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(a) (b) 



(c) 


Figure 12.43: Grasped object. 


contacts, while contact C is a point contact with friction cone defined by angle 
(3. /3 lies in the range 0 < f3 < Angles a and 9 are defined as shown in Fig¬ 
ure 12.43(c), with 6 satisfying a < 9 < f — a. Find the range of values for angles 
/3 and 9 such that the grasp is force closure. {Hint: Nguyen’s Theorem is useful). 


28 . Consider a planar donut-like rigid object shown in Figure 12.44(a). 

(a) Does there exist any set of four frictionless point contacts that makes this 
grasp force closure? 

(b) Point contacts B and C, both with friction coefficient p > 0, are placed as 
shown in Figure 12.44(b). The position of contact C is variable, so that the 
angle 9 = ZBOC can vary. Find the range of 9 that makes this grasp force 
closure in terms of p. 

29 . In Figure 12.45, the cross-shaped object with four circular holes is re- 
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B 



B and C for Problem (b) 
Figure 12.44: Grasped donut 



Figure 12.45: A cross-shaped object with holes. 


strained from the interior by four point contacts (A, B,C, D). The distance 
from the center of the cross to each hole is L 1 and the radius of each hole is r. 
Angles 9i, 9 2 , 9 3 , 64 are defined as shown. 

(a) Let 64 = ? 7 r, 9 2 = ^7r, 0 3 = | 7 r, 04 = | 7 T and assume A,B,C,D are 
frictionless point contacts. Is this grasp form closure? 

(b) Suppose 0i = 0, 03 = 0, and A,B,C,D are frictionless point contacts. 
For what values of 9 2 and 04 is the grasp form closure? 

(c) Now suppose there is no contact at point D(there are only 3 contact points). 
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A and C are frictionless point contacts, whereas B is a point contact with fric¬ 
tion coefficient /i = 1 as shown in Figure 12.45(b). Suppose L is large enough 
to ignore r (i.e., L r) and 62 = 0. Find all values of 9\ and O 3 that makes 
the grasp force closure. 



(a) Grasp for Problem (a) 



Figure 12.46: Planar grasp. 


30 . (a) For the planar grasp of Figure 12.46(a), assume contact C is frictionless, 
while contacts A and B have friction (assume p=l). Determine whether this 
grasp is force closure. 

(b) For the planar grasp of Figure 12.46(b), assume contacts A and B are 
frictionless, while contact C has a friction cone of angle /?. Find the range of 
values of (3 that makes this grasp force closure. 



Figure 12.47: Grasped object. 




































12.6. Exercises 


449 


31 . The object of Figure 12.47 consists of 4 semicircular petals with a circular 
hole in the middle. Assume the radius is R for all semicircles. 

(a) In the planar grasp of Figure 12.47(a), contacts A, B, C and D are frictionless. 
Determine whether the grasp is force closure. 

(b) In the planar grasp of Figure 12.47(b), contacts C and D are frictionless, 
while contact E has friction with friction coefficient /x = 1. Find the range of 
angles a and /3 such that the grasp is force closure. 



(a) 




Figure 12.48: A coin with a square hole. 


32 . Figure 12.48 shows a coin with a square hole in the middle grasped by 
several point contacts as shown. 

(a) Suppose the coin is grasped by two frictionless point contacts and one point 
contact with friction coefficient fi = 1 placed on the inside edge as shown in 
Figure 12.48(a). Determine whether the grasp is force closure. 
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(b) Suppose the coin is now grasped by four frictionless point contacts as shown 
in Figure 12.48(b), three of which are placed on the inside edge at the locations 
shown. The fourth point contact can be placed anywhere on the outside edge. 
Determine all the contact point locations on the outside edge that makes this 
grasp force closure. 

(c) Three frictionless point contacts are placed on the inside edge as shown 
in Figure 12.48(c). Suppose any number of frictionless point contacts can be 
placed on the outside edge. Find the minimum number of outside edge point 
contacts that makes this grasp force closure. 

(d) Is it possible to construct a force closure grasp using exactly two frictionless 
point contacts? Explain your answer. 



33. A pie-shaped planar rigid body of radius R = 1 is grasped by four point 
contacts B(t 2 ), C{ 6 1 ), and D(9 2 ) as shown in Figure 12.49. The contact 

locations are constrained as follows: 

0 < ti,t 2 < 1, 0<6 »i<|, \ <02< \- 

(a) Suppose all the point contacts are frictionless, and t\ = 1/2, t 2 = 1/2, 
9\ = 7t/6, 9 2 = 7t/3. Use the convex hull test to determine if this grasp is force 
closure. 

(b) Suppose all the point contacts are frictionless, but that the contact locations 
for A(ti), B(t 2 ), C(9\) are arbitrarily given. Does there always exist a feasible 
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contact location for 0 ( 62 ) that makes the grasp force closure? If yes, explain 
your answer based on the convex hull test. If no, provide a counterexample and 
carefully explain your reasoning. 

(c) Contact D is now eliminated, so that the body is now grasped by the three 
contacts A, B , and C only. Assume that point contacts A and B have friction 
coefficient n = |, with locations fixed at t\ = 1/2 and t 2 = 1/2, respectively. 
Assume point contact C is frictionless, with its location determined by angle 6 \. 
Find the range of values for 9\ such that the grasp is always force closure. 



Figure 12.50: A Planar object. 


34 . The planar object of Figure 12.50 is restrained by several point contacts. 
A, B, D } and E are frictionless point contacts, while C is a frictional point 
contact with /i = 1. Let 0 < L < 2. 

(a) Suppose the object is restrained by point contacts A and E only. Is this 
grasp force closure? 

(b) Suppose the object is restrained by point contacts B, C, and D only. For 
what range of values of L is the grasp force closure? 

(c) Suppose the object is restrained by point contacts A, B , and C only. Is this 
grasp force closure? 

35 . Come up with a formula, as a function of n, for the minimum friction 
coefficient needed for a two-fingered force-closure grasp of an n-sided regular 
polygon, where n is odd. Assume the fingers can only make contact with the 
edges, not the vertices. If the fingers could also contact the vertices, how does 
your answer change? You can assume the fingers are circular. 

36 . Consider a table at rest, supported by four legs in frictional contact with the 
floor. The normal forces provided by each leg are not unique; there is an infinite 
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Figure 12.51: A zero thickness rod supported by a single contact. 



Figure 12.52: A frictionless finger pushes a box to the right. Gravity acts 
downward. Does the box slide flat against the table, does it tip over the right 
corner, or does it slide and tip on the right corner? 


set of solutions to the normal forces yielding force balance with gravity. What 
is the dimension of the space of normal force solutions? What is the dimension 
of the space of contact force solutions if we include tangential frictional forces? 

37 . A thin rod in gravity is supported from below by a single stationary contact, 
shown in Figure 12.51. You can place one more contact anywhere else on the 
top or the bottom of the rod. Indicate all places you can put this contact while 
balancing the gravitational force. Use moment labeling to justify your answer. 
Prove the same using algebraic force balance, and comment on the magnitude 
of the normal forces depending on the location of the second contact. 

38 . A frictionless finger begins pushing a box over a table (Figure 12.52). There 
is friction between the box and the table, as indicated in the figure. There are 
three possible contact modes between the box and the table: either the box 
slides to the right flat against the table, or it tips over the right corner, or it tips 
over the right corner while the the corner also slides to the right. Which actually 
occurs? Assume quasistatic force balance and answer the following questions, 
(a) For each of the three contact modes, draw the moment labeling regions 
corresponding to the table friction cone edges active in that contact mode, (b) 
For each moment labeling drawing, determine whether the pushing force plus the 
gravitational force can be quasistatically balanced by the support forces. From 
this, determine which contact mode actually occurs, (c) Graphically show a 
support friction cone for which your answer changes. 
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(x L , y ) 



Figure 12.53: One body leaning on another (Exercise 39). 


39 . In Figure 12.53, body 1 , of mass mi with center of mass at {xi,y±), leans 
on body 2, of mass m 2 with center of mass at (X 2 , 3 / 2 )- Both are supported 
by a horizontal line, and gravity acts downward. The friction coefficient at all 
four contacts (one at (0,0), one at {xl,v), one at (xl, 0), and one at (xr,0)) is 
H > 0. We want to know if it is possible for the assembly to stay standing by 
some choice of contact forces within the friction cones. Write the six equations 
of force balance for the two bodies in terms of the gravitational forces and the 
contact forces, and express the conditions that must be satisfied for it to be 
possible for this assembly to stay standing. How many equations and unknowns 
are there? 

40 . Write a program that accepts a set of contacts acting on a planar part and 
determines whether the part is in first-order form closure. 

41 . Write a program that accepts a set of contacts acting on a spatial part and 
determines whether the part is in first-order form closure. 

42 . Write a program that accepts a friction coefficient and a set of contacts 
acting on a planar part and determines whether the part is in force closure. 

43 . Write a program that accepts a friction coefficient and a set of contacts 
acting on a spatial part and determines whether the part is in force closure. 
Use a polyhedral approximation to the friction cone at each contact point that 
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underestimates the friction cone and that has four facets. 

44 . Write a program to simulate the quasistatic meter stick trick of Exam¬ 
ple 12.7. The program takes as input the initial ^-position of the left finger, the 
right finger, and the stick’s center of mass; the constant speed x of the right 
finger (toward the left finger); and the static and kinetic friction coefficients, 
where g s > /ik- The program should simulate until the two fingers touch, or 
until the stick falls. The program should plot the position of the left finger 
(which is constant), the right finger, and the center of mass as a function of 
time. Include an example where g s = gk, an example where g s is only slightly 
larger than g^, and an example where p s is much larger than gy. 

45 . Write a program that determines if a given assembly of planar parts can 
remain standing in gravity. Gravity g acts in the —y direction. The assembly 
is described by m bodies, n contacts, and the friction coefficient g , all entered 
by the user. Each of the m bodies is described by its mass rip and the ( Xi,yi ) 
location of its center of mass. Each contact is described by the index i of each 
of the two bodies involved in the contact and the unit normal direction (defined 
as into the first body). If the contact has only one body involved, the second 
body is assumed to be stationary (e.g., ground). The program should look for 
a set of coefficients kj > 0 multiplying the friction cone edges at the contacts 
(if there are n contacts, then there are 2 n friction cone edges and coefficients) 
such that each of the m bodies is in force balance, considering gravity. Except in 
degenerate cases, if there are more force balance equations (3 to) than unknowns 
(277.), there is no solution. In the usual case where 2 n > 3m, there is a family 
of solutions, meaning that the force at each contact cannot be known with 
certainty. 

One approach is to have your program generate an appropriate linear pro¬ 
gram, and use the programming language’s built-in linear programming solver. 

46 . This is a generalization of the previous exercise. Now, instead of sim¬ 

ply deciding whether the assembly stays standing for a stationary base, the 
base moves according to a trajectory specified by the user, and the program 
determines whether the assembly can stay together during the trajectory (i.e., 
whether sticking contact at all contacts allows each body to follow the specified 
trajectory). The three-dimensional trajectory of the base can be specified as a 
polynomial in (x(t), y{t) : for a base reference frame defined at a particular 

position. For this problem, you also need to specify the scalar moment of inertia 
about the center of mass for each part. You may find it convenient to express 
motion and forces (gravitational, contact, inertial) in the frame of each part and 
solve for the dynamics in the body frames. Your program should check for sta¬ 
bility (all contact normal forces are nonnegative while satisfying the dynamics) 
at finely spaced discrete points along the trajectory. It should return a binary 
result: the assembly can be maintained at all points along the trajectory, or 
not. 


Chapter 13 

Wheeled Mobile Robots 


A kinematic model of a mobile robot governs how wheel speeds map to robot 
velocities, while a dynamic model governs how wheel torques map to robot 
accelerations. In this chapter, we ignore dynamics and focus on kinematics. 
We also assume that the robots roll on hard, flat, horizontal ground without 
skidding (i.e., no tanks or skid-steered vehicles). The mobile robot is assumed 
to have a single rigid-body chassis (not articulated like tractor-trailers) with a 
configuration T sb £ SE( 2) representing a chassis-fixed frame {b} relative to a 
fixed space frame {s} in the horizontal plane. In this chapter we represent T sb 
by the three coordinates q = (cf>,x,y). We also usually represent the velocity of 
the chassis as the time derivative of the coordinates, q = (<j>, x, y). Occasionally 
it will be convenient to refer to the chassis’ planar twist V b = (wbz,Vbx,Vby) 
expressed in {b}, where 
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(13.1) 


(13.2) 


This chapter covers kinematic modeling, motion planning, and feedback con¬ 
trol for wheeled mobile robots, and concludes with a brief introduction to mobile 
manipulation. 


13.1 Types of Wheeled Mobile Robots 

Wheeled mobile robots may be classified in two major categories, omnidi¬ 
rectional and nonholonomic. Omnidirectional mobile robots have no equality 
constraints on the chassis velocity q = (<p,x,y), while nonholonomic robots are 
called “nonholonomic” because the chassis is subject to a single Pfaffian ve¬ 
locity constraint A(q)q = 0 (see Chapter 2.4 for an introduction to Pfaffian 
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Figure 13.1: Left: A typical wheel that rolls without sideways slip (here a 
unicycle wheel). Middle: An omniwheel. Right: A mecanum wheel. Omniwheel 
and mecanum wheel images from www.vexrobotics.com, used with permission. 


constraints). For a car-like robot, this constraint is that the car cannot move 
directly sideways. Despite this velocity constraint, the car can reach any (<(>, x, y) 
configuration in the obstacle-free plane. In other words, the velocity constraint 
cannot be integrated to an equivalent configuration constraint, and therefore it 
is a nonholonomic constraint. 

Whether a wheeled mobile robot is omnidirectional or nonholonomic depends 
on the type of wheels it employs (Figure 13.1). Nonholonomic mobile robots 
employ “typical” wheels, such as one you might find on your car: it rotates 
about an axle perpendicular to the plane of the wheel at the wheel’s center, and 
optionally it can be steered by spinning the wheel about an axis perpendicular 
to the ground at the contact point. The wheel rolls without sideways slip, which 
is the source of the nonholonomic constraint on the robot’s chassis. 

Omnidirectional wheeled mobile robots typically employ either omniwheels 
or mecanum wheels . 1 An omniwheel is a typical wheel augmented with rollers 
on the outer circumference of the wheel. These rollers spin freely about axes in 
the plane of the wheel and tangent to the wheel’s outer circumference, and they 
allow sideways sliding while the wheel drives forward and backward without 
slip in that direction. Mecanum wheels are similar, except the spin axes of the 
circumferential rollers are not in the plane of the wheel (see Figure 13.1). The 
sideways sliding allowed by omniwheels and mecanum wheels ensure that there 
are no velocity constraints on the robot chassis. 

Omniwheels and mecanum wheels are not steered, only driven forward or 
backward. Because of the small diameter rollers, omniwheels and mecanum 
wheels work best on hard, flat ground. 

Because the issues in modeling, motion planning, and control of wheeled 
mobile robots depend intimately on whether the robot is omnidirectional or 
nonholonomic, we treat these two cases separately in the following sections. 

1 These types of wheels are often called “Swedish wheels,” as they were invented by Bengt 
lion working at the Swedish company Mecanum AB. The usage of, and the differentiaiton 
between, the terms “omniwheel,” “mecanum wheel,” and “Swedish wheel” is not completely 
standard, but here we use one popular choice. 
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Figure 13.2: (Left) A mobile robot with three omniwheels. Also shown for one 
of the omniwheels is the direction that the wheel can freely slide due to the 
rollers, as well as the direction the wheel rolls without slipping when driven 
by the wheel motor. (Top image from www.superdroidrobots.com, used with 
permission.) (Right) The KUKA youBot mobile manipulator system, which uses 
four mecanum wheels for its mobile base. (Top image used with permission.) 

13.2 Omnidirectional Wheeled Mobile Robots 

13.2.1 Modeling 

To achieve an arbitrary three-dimensional chassis velocity q = (4>,x,y), an om¬ 
nidirectional mobile robot must have at least three wheels, since each wheel has 
only one motor (controlling its forward/backward velocity). Figure 13.2 shows 
two omnidirectional mobile robots, one with three omniwheels and one with 
four mecanum wheels. Also shown are the motions that are obtained by driving 
the wheel motors as well as the free sliding motions allowed by the rollers. 

Two important questions in kinematic modeling are: 

(i) Given a desired chassis velocity q , at what speeds must the wheels be 
driven? 

(ii) Given limits on the individual wheel driving speeds, what are the limits 
on the chassis velocity g? 

To answer these questions, we need to understand the wheel kinematics 
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free “sliding” direction 



driven component = 


v x + v y tan y 



Figure 13.3: (Left) The direction of the wheel motion due to driving the wheel’s 
motor and the direction that its rollers allow it to slide freely. For an omniwheel, 
7 = 0, and for a mecanum wheel, typically 7 = ±45°. (Right) The driven and 
free-sliding speeds for the wheel velocity v = ( v x ,v y ) expressed in the wheel 
frame x w -y wJ where the x w -axis is aligned with the forward driving direction. 


illustrated in Figure 13.3. In a frame x w -y w at the center of the wheel, the 
linear velocity of the center of the wheel is written v = (v x ,v y ), which satisfies 
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(13.3) 


where 7 denotes the angle at which free “sliding” occurs (allowed by the passive 
rollers on the circumference of the wheel), thrive is the driving speed, and t^nde is 
the sliding speed. For an omniwheel, 7 = 0 , and for a mecanum wheel, typically 
7 = ±45°. Solving Equation (13.3), we get 


^drive = V x + Vy tan 7 
Wslide =Vy/ COS 7. 


Letting r be the radius of the wheel and u be the driving angular speed of the 
wheel, 

u = drlve = - ( v x + v v tan 7 ). (13-4) 

TV 

To derive the full transformation from a chassis velocity q = ( (f >, x , y ) to 
the driving angular speed Ui for wheel *, refer to the notation illustrated in 
Figure 13.4. The chassis frame {b} is at q = (cj),x,y) in the fixed space frame 
{s}. The center of the wheel and its driving direction are given by (/3i,Xi,yi) 
expressed in {b}, the wheel’s radius is , and the wheel’s sliding direction is 
given by 7 *. Then Ui is related to q by 
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(13.5) 
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driving direction 



Figure 13.4: The fixed space frame {s}, a chassis frame {b} at (</),x,y) in {s}, 
and wheel i at (xi,yi) with a driving direction /3j, both expressed in {b}. The 
sliding direction of wheel i is defined by %. 


Reading from right to left, the first transformation expresses q as 14; the next 
transformation produces the linear velocity at the wheel in {b}; the next trans¬ 
formation expresses this linear velocity in the wheel frame x w -y w ; and the final 
transformation calculates the driving angular velocity using Equation (13.4). 

Evaluating Equation (13.5) for hi(<f >), we get 


hi{<j>) 
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U cos 7 i 


Xi sin (Pi + 7 i) - yi cos(/3j + 7 *) 
cos {Pi + 7 i + 4>) 
sin (/3i + 7 i + 4>) 


(13.6) 


For an omnidirectional robot with m > 3 wheels, the matrix H(cf>) £ M mx3 
mapping a desired chassis velocity q £ R 3 to the vector of wheel driving speeds 
u £ R m is constructed by stacking the m rows hi (</>): 


u = H{<j))q 
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(13.7) 


We can also express the relationship between u and the body twist V&. This 
mapping does not depend on the chassis orientation 1 p: 
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wheel 1 
Yi = 0, Pj= 0 



wheel 4 wheel 1 
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wheel 3 wheel 2 


Figure 13.5: Kinematic models for mobile robots with three omniwheels and 
four mecanum wheels. The radius of all wheels is r and the driving direction 
for each of the mecanum wheels is /3j = 0 . 


The wheel positions and headings in {b}, and their free slid¬ 

ing directions 7 $, must be chosen so that H( 0) is rank 3. For example, if we 
constructed a mobile robot of omniwheels whose driving directions and sliding 
directions were all aligned, the rank of H( 0) would be two, and there would be 
no way to controllably generate translational motion in the sliding direction. 

In the case m > 3, such as the four-wheeled youBot of Figure 13.2, choosing 
u such that Equation (13.8) is not satisfied for any V b £ R 3 implies that the 
wheels must skid in their driving directions. 

Using the notation in Figure 13.5, the kinematic model of the mobile robot 
with three omniwheels is 
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and the kinematic model of the mobile robot with four mecanum wheels is 
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(13.10) 


For the mecanum robot, to move in +Xb, all wheels drive forward at the same 
speed; to move in +yb; wheels 1 and 3 drive backward and wheels 2 and 4 drive 
forward at the same speed; and to rotate in the counterclockwise direction, 
wheels 1 and 4 drive backward and wheels 2 and 3 drive forward at the same 
speed. Note that the robot chassis is capable of the same speeds in the forward 
and sideways directions. 
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Figure 13.6: (Top row) Regions of feasible body twists V for the three-wheeled 
(left) and four-wheeled (right) robots of Figure 13.5. Also shown for the three¬ 
wheeled robot is the intersection with the u>b z = 0 plane. (Bottom row) The 
bounds in the Wb z = 0 plane (translational motions only). 


If the driving angular velocity of wheel i is subject to bounds |itj| < Ui jmax , 

he., 

max — ^i (0)V„ — 'U'i, max 5 

two parallel constraint planes defined by —Ui t max = ft* ( 0)14 and Mi imax = 
hj(Q)Vh are generated in the three-dimensional space of body twists. Any V 5 
between these two planes is a feasible chassis body twist according to wheel i. 
while any Vb outside this slice is infeasible. The normal direction to the con¬ 
straint planes is hf ( 0 ), and the points on the planes closest to the origin are 
-u i}max hf (0)/\\hi(0)\\ 2 and u i}lnax hf ( 0 )/||/ii( 0 )|| 2 . 

If the robot has m wheels, then the region of feasible body twists V is 
bounded by the m pairs of parallel constraint planes. The region V is therefore 
a convex three-dimensional polyhedron, with the origin (zero twist) in the center 
and 2 m faces. Visualizations of the 6 -sided and 8 -sided regions V for the three¬ 
wheeled and four-wheeled models in Figure 13.5 are shown in Figure 13.6. 

13.2.2 Motion Planning 

Since omnidirectional mobile robots are free to move in any direction, any of 
the trajectory planning methods for kinematic systems in Chapter 9, and most 
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of the motion planning methods of Chapter 10, can be adapted. 

13.2.3 Feedback Control 

Given a desired trajectory qa(t), we can adopt the feedforward plus proportional 
feedback linear controller (11.1) of Chapter 11 to track the trajectory: 

q C om(t) = q d (t) + K p (q d (t) - q(t)), (13.11) 

where K p £ R 3x3 is positive definite and q(t) is an estimate of the actual con¬ 
figuration derived from sensors. Then q com (t) can be converted to commanded 
wheel driving velocities u com (t) using Equation (13.7). 

13.3 Nonholonomic Wheeled Mobile Robots 

In Chapter 2.4, the k Pfaffian velocity constraints acting on a system with 
configuration q £ R” were written as A(q)q = 0, where A(q) £ R kxn . Instead 
of specifying the k directions in which velocities are not allowed, we can write 
the allowable velocities of a kinematic system as a linear combination of n — k 
velocity directions. This representation is equivalent, and it has the advantage 
that the coefficients of the linear combinations are precisely the controls available 
to us. We will see this representation in the kinematic models below. 

By the title of this section, we are implying that the velocity constraints 
are not integrable to equivalent configuration constraints. We will establish this 
formally in Section 13.3.2. 

13.3.1 Modeling 

13.3.1.1 The Unicycle 

The simplest wheeled mobile robot is a single upright rolling wheel, or unicycle. 
Let r be the radius of the wheel. We write the configuration of the wheel as 
q = ((f>,x,y,9) T , where (x,y) is the contact point, <j> is the heading direction, 
and 9 is the rolling angle of the wheel (Figure 13.7). The configuration of the 
“chassis” (e.g., the seat of the unicycle) is {<f>,x,y). The kinematic equations of 
motion are 
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= G(q)u = gi(q)ui + g 2 {q)u 2 . (13.12) 


The control inputs are u = (ui 1 u 2 ) t 1 with ui the forward-backward angular 
speed and u 2 the angular turning speed. The controls are subject to the con¬ 
straints -Ml, max <Ui< Ui, max and -U2,max < U 2 < U 2 , max- 
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Figure 13.7: A wheel rolling on a plane without slipping. 
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Figure 13.8: The vector field (x,y) = (—y,x). 


The vector-valued functions gi(q) £ R 4 are the columns of the matrix G{q) 1 
and these are called the tangent vector fields (also called the control vector 
fields or simply velocity vector fields) over q associated with the controls it,; = 1. 
Evaluated at a specific configuration q, gi(q) is a tangent vector (or velocity 
vector) of the tangent vector field. 

An example of a vector field on R 2 is illustrated in Figure 13.8. 

All of our kinematic models of nonholonomic mobile robots will have the 
form q = G(q)u , as in (13.12). Three things to notice about these models are: 
(1) there is no drift—zero controls mean zero velocity; (2) the vector fields gj(q) 
are generally functions of the configuration g; and (3) q is linear in the controls. 

Since we are not usually concerned with the rolling angle of the wheel, we 
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x 


Figure 13.9: A diff-drive robot consisting of two typical wheels and one ball 
caster wheel, shaded gray. 


can drop the fourth row from (13.12) to get the simplified control system 
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13.3.1.2 The Differential-Drive Robot 

The differential-drive robot, or diff-drive, is perhaps the simplest wheeled 
mobile robot architecture. A diff-drive robot consists of two independently 
driven wheels of radius r that rotate about the same axis, as well as one or more 
caster wheels, ball casters, or low-friction sliders that keep the robot horizontal. 
Let the distance between the wheels be 2d and choose the (x, y) reference point 
halfway between the wheels (Figure 13.9). Writing the configuration as q = 
{4>, x, y, 6<l, 0r) t , where and 0r are the rolling angles of the left and right 
wheels, respectively, the kinematic equations are 
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where is the angular speed of the left wheel and Mr is the angular speed of 
the right wheel. Positive angular speed of each wheel corresponds to forward 
motion at that wheel. The controls at each wheel are taken from the interval 

[ ^maxi ^max] ■ 

Since we are not usually concerned with the rolling angles of the two wheels, 
we can drop the last two rows to get the simplified control system 
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Figure 13.10: The two front wheels of a car are steered at different angles using 
Ackermann steering so that all wheels roll without slipping. The car is shown 
executing a turn at its minimum turning radius r m i n . 


Two advantages of a diff-drive robot are its simplicity (typically a motor 
attached directly to the axle of each wheel) and high maneuverability (the robot 
can spin in place by rotating the wheels in opposite directions). Casters are often 
not appropriate for outdoor use, however. 


13.3.1.3 The Car-Like Robot 


The most familiar wheeled vehicle is a car, with two steered front wheels and 
two fixed-heading rear wheels. To prevent slipping of the front wheels, they are 
steered using Ackermann steering, as illustrated in Figure 13.10. The center 
of rotation of the car’s chassis lies on the line passing through the rear wheels 
at the intersection with the perpendicular bisectors of the front wheels. 

To define the configuration of the car, we ignore the rolling angles of the four 
wheels and write q = (cp,x,y,ip) T , where (a :,y) is the location of the midpoint 
between the rear wheels, <p is the car’s heading direction, and ip is the steering 
angle of the car, defined at the virtual wheel at the midpoint between the front 
wheels. The controls are the forward speed v of the car at its reference point 
and the angular speed w of the steering angle. The car’s kinematics are 
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where i is the wheelbase between the front and rear wheels. The control v is 
limited to a closed interval [u m i n > r m ax] where u m ; n < 0 < v ma , x , the steering rate 
is limited to [—u> max , ic max ] with w max > 0, and the steering angle ip is limited 

to [^min, Ip max] • 
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The kinematics (13.16) can be simplified if the steering control is actually 
the steering angle il> and not its rate w. This assumption is justified if the 
steering rate limit M) max is high so that the steering angle can be changed nearly 
instantaneously by a lower-level controller. In this case, if) is eliminated as a 
state variable, and the car’s configuration is simply q = (< t>,x,y) T . We use the 
control inputs (u,w), where v is still the car’s forward speed and oj is now its 
rate of rotation. These can be converted to the controls ( v,i/j ) by the relations 

v = v, V’ = tan -1 ( — J . (13.17) 


The constraints on the controls (u,w) due to the constraints on ( v,il >) take a 
somewhat complicated form, as we see shortly. 

The simplified car kinematics can now be written 
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(13.18) 


The nonholonomic constraint associated with this system can be derived by 
using one of the equations from (13.18) 


x = v cos (f> 
y = v sin (f> 

to solve for v, then plugging the result into the other equation to get 
A(q)q = [ 0, sin cj), — cos </> ] q = x sin (j> — y cos (j) = 0. 


13.3.1.4 Canonical Simplified Model for Nonholonomic Mobile Robots 

The kinematics (13.18) are a canonical simplified model for nonholonomic mobile 
robots. Using control transformations such as (13.17), the simplified unicycle 
kinematics (13.13) and the simplified differential-drive kinematics (13.15) can 
also be expressed in this form. The control transformation for the simplified 
unicycle kinematics (13.13) is 

v 

Mi = -, m 2 = OJ 
r 

and the transformation for the simplified diff-drive kinematics (13.15) is 

v — ixd v + ud 

ml = -, mr =-. 

r r 

With these input transformations, the only difference between the simplified 
unicycle, diff-drive robot, and car kinematics are the control limits on ( v,co ). 
These are illustrated in Figure 13.11. 

We can use the two control inputs ( v,co ) in the canonical model (13.18) to 
directly control the two components of the linear velocity of a reference point 
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Figure 13.11: The (v,co) control sets for the simplified unicycle, diff-drive robot, 
and car kinematics. The car’s control set illustrates that it is incapable of 
turning in place. The angle of the sloped lines in the car’s bowtie control set 
is determined by the car’s minimum turning radius. If the car has no reverse 
gear, only the right half of the bowtie is available. 



Figure 13.12: The point P is located at ( x r ,y T ) in the chassis-fixed frame {b}. 


P fixed to the robot chassis. This is useful when a sensor is located at P, for 
example. Let ( Xp,yp ) be the coordinates of P in the world frame, and (x T ,y T ) 
be the (constant) coordinates in the chassis frame {b}, with Xb in the robot’s 
heading direction (Figure 13.12). To find the controls (v,w) needed to achieve 
a desired world-frame motion ( Xp , yp), we first write 

x P 

yp 

Differentiating, we get 


cos <p — sin ( 
sin d> cos < 



x r 


Vr 


(13.19) 


Xp 


. yp 



sin <j) — cos <f> 


X r 

cos <j> — sin <f> 


. Vr _ 


(13.20) 


Substituting w for <f> and (v cos <f>, v sin <j>) for (x, y) and solving, 


we get 


V 

1 

x T cos <j> — y T sin cf) 

x r sin 4> + y r cos 4> 


Xp 

UJ 

x r 

— sin cf> 

COS (j) 


yp 


(13.21) 


This equation may be read as ( v,oj) t = J~ 1 {q){xp,yp ) T , where J(q) is the 
Jacobian relating (v, u) to the world-frame motion of P. Note that the Jacobian 
J(q) is singular when P is chosen on the line x T = 0. Points on this line, such as 
at the midway point between the wheels of a diff-drive robot or the rear wheels 
of a car, can only move in the heading direction of the vehicle. 
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13.3.2 Controllability 

Feedback control for an omnidirectional robot is simple, as there is a set of wheel 
driving speeds for any desired chassis velocity q (Equation (13.7)). In fact, if 
the goal of the feedback controller is simply to stabilize the robot to the origin 
q = (0,0,0), not trajectory tracking as in the control law (13.11), we could use 
the even simpler feedback controller 


<?com(i) = - Kq(t ) (13.22) 

for any positive definite K. The feedback gain matrix —K acts like a spring to 
pull q to the origin, and Equation (13.7) is used to transform q CO m(t) to u com (t). 
The same type of “linear spring” controller could be used to stabilize the point 
P on the canonical nonholonomic robot (Figure 13.12) to (xp, yp) = (0, 0) since, 
by Equation (13.21), any desired ( Xp,yp ) can be achieved by controls (u,w). 2 

In short, the kinematics of the omnidirectional robot, as well as the kine¬ 
matics of the point P for the nonholonomic robot, can be rewritten in the 
single-integrator form 

x = v, (13.23) 

where x is the configuration we are trying to control and v is a “virtual control” 
that is actually implemented using the transformations in Equation (13.7) for 
an omnidirectional robot or Equation (13.21) for the control of P by a nonholo¬ 
nomic robot. Equation (13.23) is a simple example of the more general class of 
linear control systems 

x = Ax + Bv (13.24) 

which are known to be linearly controllable if the Kalman rank condition 

is satisfied: 


rank([B AB A 2 B ... A n ~ 1 B]) = dim(x) = n, 

where x £ R ra ,i' £ M m ,7l £ M. nXn ,B £ R nxm . In Equation (13.23), A = 0 
and B is the identity matrix, trivially satisfying the rank condition for linear 
controllability, since m = n. This implies the existence of the simple linear 
control law 

v = —Kx, 

as in Equation (13.22), to stabilize the origin. 

There is no linear controller that can stabilize the full chassis configuration 
to q = 0 for a nonholonomic robot, however; the nonholonomic robot is not 
linearly controllable. More than that, there is no controller that is a continuous 
function of q that can stabilize q = 0. This fact is embedded in the following 
well known result, which we state without proof. 

Theorem 13.1. A system q = G(q)u with rank(G(0)) < dim(gr) cannot be 
stabilized to q = 0 by a continuous time-invariant feedback control law. 

2 For the moment we ignore the different constraints on (v, cj) for the unicycle, diff-drive 
robot, and car-like robot, as they do not change the main point. 
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STLA STLC 


Figure 13.13: Illustrations of small-time local accessibility (STLA) and small¬ 
time local controllability (STLC) in a two-dimensional space. The shaded re¬ 
gions are the reachable sets without leaving the neighborhood W(q). 


This theorem applies to our canonical nonholonomic robot model, since the rank 
of G(q) is two everywhere (there are only two control vector fields), while the 
chassis configuration is three dimensional. 

For nonlinear systems of the form q = G(q)u , there are other notions of 
controllability, however. We consider a few of these next, and show that even 
though the canonical nonholonomic robot is not linearly controllable, it still 
satisfies other important notions of controllability. In particular, the velocity 
constraint does not integrate to a configuration constraint. 

13.3.2.1 Definitions of Controllability 

We begin with some standard definitions of nonlinear controllability. 

Definition 13.1. A robot is controllable from q if, for any q goa i, there exists 
a control trajectory u(t) that drives the robot from q to g g0 ai i n finite time T. 
The robot is small-time locally accessible from q if, for any time T > 0 
and any neighborhood 3 W(q), the reachable set in time less than T without 
leaving W(q) is a full-dimensional subset of the configuration space. The robot 
is small-time locally controllable (STLC) from q if, for any time T > 0 and 
any neighborhood W(q ), the reachable set in time less than T without leaving 
W{q) is a neighborhood of q. 

Small-time local accessibility (STLA) and small-time local controllability 
(STLC) are illustrated in Figure 13.13 for a two-dimensional configuration space. 
Clearly STLC at q is a stronger condition than STLA at q. If a system is 
STLC at all q , then it is controllable from any q, by patching together paths in 
neighborhoods from q to ggoai- 

For all the examples in this chapter, if a controllability property holds for 
any q , then it holds for all q , since the maneuverability of the robot does not 
change with configuration. 

Consider the examples of a car and a forward-only car with no reverse gear. 
A forward-only car is STLA, as we will see shortly, but it is not STLC; if it is 
confined to a tight space (a small neighborhood W ), it cannot reach configu¬ 
rations directly behind its initial configuration. On the other hand, a car with 

3 A neighborhood W(q) of a configuration q is any full-dimensional set of configuration 
space containing q in the interior. For example, the set of configurations in a ball of radius 
r > 0 centered at q (i.e., all qi satisfying ||gj, — q\\ < r) is a neighborhood of q. 
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a reverse gear is STLC. Both cars are controllable in the obstacle-free plane, 
because even a forward-only car can drive anywhere. 

If there are obstacles in the plane, there may be some free space configura¬ 
tions the forward-only car cannot reach that the STLC car can reach. (Consider 
an obstacle directly in front of the car, for example.) If the obstacles are all 
defined as closed subsets of the plane containing their boundaries, the STLC 
car can reach any configuration in its connected component of the free space. 

It is worth thinking about this last statement for a moment. All free configu¬ 
rations have collision-free neighborhoods, since the free space is defined as open 
and the obstacles are defined as closed (containing their boundaries). Therefore 
it is always possible to maneuver in any direction. This means that if the length 
of your car is shorter than the available parking space, you can parallel park 
into it, even if it takes a long time! 

If any of the controllability properties holds (controllable, STLA, or STLC), 
then the reachable configuration space is full dimensional, and therefore any 
velocity constraints on the system are nonholonomic. 

13.3.2.2 Controllability Tests 

Consider a driftless, linear-in-the-control (“control affine”) system 

m 

q = G(q)u = < 7 i(g)«j, q € R", u € U C R m , m < n, (13.25) 

i=i 

generalizing the canonical nonholonomic model where n = 3 and m = 2. The set 
of feasible controls is U C R m . For example, the control sets U for the unicycle, 
diff-drive, car-like, and forward-only car-like robots are shown in Figure 13.11. 
In this chapter we consider two types of control sets U\ those whose positive 
linear span is i.e., pos(W) = R m , such as the control sets for the unicycle, 
diff-drive robot, and car in Figure 13.11, and those whose positive linear span 
does not cover R m but whose linear span does, i.e., span(W) = K m , such as the 
control set for the forward-only car in Figure 13.11. 

Local controllability properties (STLA or STLC) of (13.25) depend on the 
non-commutativity of motions along the vector fields <?,. Let F® 4 (q) be the 
configuration reached by following the vector field gt for time e starting from 
q. Then gi(q) and gj(q ) commute if F<f J (F e 9i (q)) = F® 4 (F® 3 (g)), i.e., the order 
of following the vector fields does not matter. If they do not commute, i.e., 
Ff 3 (F® 4 (q)) — F® 4 (F? 3 (q)) ^ 0, then the order of application of the vector fields 
affects the final configuration. In addition, if we define the noncommutativity 
as 

A q = F* (F® 4 (q)) - F® 4 (Fj! 1 (q)) for small e, 

if Aq is in a direction that cannot be achieved directly by any of the other vector 
fields gk, then switching between gi and gj can create motion in a direction not 
present in the original set of vector fields. A familiar example is parallel parking 
a car: there is no vector held corresponding to direct sideways translation, but 
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by alternating forward and backward motion along two different vector fields, 
it is possible to create a net motion to the side. 

To approximately calculate q( 2e) = Fe 1 (F^ i (q( 0))) for small e, we use a 
Taylor expansion and truncate the expansion at 0(e 3 ). We start by following 
gi for time e, and use the fact that q = gi(q) and q = -^q = g l (q): 


q(e)=q(0) + em+l^ 2 m + O(e 3 ) 

= q( 0 ) + egi(q{ 0 )) + - e 2 -^g i (q(0)) + 0 (e 3 ). 

Now, after following gj for time e: 

q(2e) = q(e) + egj{q(e)) + ^^^(e)) + 0{e 3 ) 

= 9(0) + e9i(9(0)) + \ e2 ^q9z(l(0)) + 

£9j(9( 0) + e9i(9(0))) + \ e2d ^q9j(Q(0)) + 0(e 3 ) 

= 9 ( 0 ) + £ 91 ( 9 ( 0 )) + \ e2 ^9i(q{ 0 )) + 

e 9i(9( 0)) + e 2 -^-gi(q( 0)) + 2 e 2 _ ^ 9 j'( 9 ( 0 )) + 0(e 3 ). (13.26) 

Note the presence of the £ 2 y)(y 9 i term, the only term that depends on the order 
of the vector fields. Using the expression (13.26), we can calculate the noncom¬ 
mutativity 

A 9 = F»{F«{q))-F?{F*{q)) = e 2 {^ 9i - ^g^j (q(0))+O(e 3 ). (13.27) 

In addition to measuring noncommutativity, A q is also the net motion (to order 
e 2 ) obtained by following gi for time e, then gj for time e, then —g, t for time e, 
then —gj for time e. 

The term — ^dq9j Equation (13.27) is important enough that we 
give it its own name: 

Definition 13.2. The Lie bracket of the vector fields 9 ^( 9 ) and g 3 ( 9 ) is 

\9i,9j](q) = (j^9i ~ ^9j^j ( 9 )- (13.28) 

This Lie bracket is the same as the Lie bracket for rigid-body motions in¬ 
troduced in Chapter 8.2.2. The only difference is that the Lie bracket in Chap¬ 
ter 8 . 2.2 was thought of as the noncommutativity of two constant twists V;, Vj 
defined at a given instant, rather than of two velocity vector fields defined over 
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all configurations q. The Lie bracket [V,, Vj] = [Vj][Vj] — [V,][V,;] from Chap¬ 
ter 8.2.2 would be identical to the expression in Equation (13.28) if the constant 
twists were represented as vector fields gi(q),gj(q) in local coordinates q. See 
Exercise 1, for example. 

The Lie bracket of two vector fields gi(q) and g 3 (q) should itself be thought 
of as a vector field [g,, g-j](q), where approximate motion along the Lie bracket 
vector field can be obtained by switching between the original two vector fields. 
As we saw in our Taylor expansion, however, motion along the Lie bracket vector 
field is slow relative to motions along the original vector fields—for small times 
e, motion of order e can be obtained in the directions of the original vector 
fields, while motion in the Lie bracket direction is only of order e 2 . This agrees 
with our common experience that moving a car sideways by parallel parking 
motions is slow relative to forward and backward or turning motions, as in the 
next example. 

Example 13.1. Consider the canonical nonholonomic robot with vector fields 
gi(q) = (0,cos(j>, smcj)) T and 32 ( 9 ) = (1,0, 0) T . The Lie bracket vector field 
53(g) = [gi,92]{q) is 

53 (g) = [51 , 52 ](g) = (j^-91 - §^ 32 ) (g) 


0 

0 

0 


0 


O 

O 

O 


' 1 ' 

0 0 0 


COS <j> 

— 

0 0 — sin (j) 


0 

0 0 0 


sincj) 


0 0 cos (j) 


0 


0 

sin (f> 
— cos (j> 


The Lie bracket direction is a sideways “parallel parking” motion, as illustrated 
in Figure 13.14. The net motion obtained by following gi for e, 92 for e, —gi 
for e, and —g 2 for e is a motion of order e 2 in this Lie bracket direction, plus a 
term of order e 3 . 

Based on the result of Example 13.1, no matter how small the maneuvering 
space is for a car with a reverse gear, it can generate sideways motion. Thus 
we have shown that the Pfaffian velocity constraint implicit in the kinematics 
q = G(q)u for the canonical nonholonomic mobile robot is not integrable to a 
configuration constraint. 

A Lie bracket [gi,gj\ is called a Lie product of degree 2, because the orig¬ 
inal vector fields appear two times in the bracket. For the canonical nonholo¬ 
nomic model, it is only necessary to consider the degree 2 Lie product to show 
that there are no configuration constraints. To test whether there are config¬ 
uration constraints for more general systems of the form (13.25), however, it 
may be necessary to consider even deeper Lie brackets, such as [gi, [gj,gk]\ or 
[gi, [gi, [gi, <jj}]], which are Lie products of degree 3 and 4, respectively. Just as 
it is possible to generate motions in Lie bracket directions by switching between 
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Figure 13.14: The Lie bracket of the forward-backward vector field gi{q) and 
the spin-in-place vector field 32(9) is a sideways vector field. 


the original vector fields, it is possible to generate motion in Lie product direc¬ 
tions of degree greater than 2. Generating motions in these directions is even 
slower than for degree 2 Lie products, however. 

The Lie algebra of a set of vector fields is defined by all Lie products of all 
degrees, including Lie products of degree 1 (the original vector fields themselves): 

Definition 13.3. The Lie algebra of a set of vector fields Q = {31,... ,g m }, 
written Lie(f7), is the linear span of all Lie products of degree 1... 00 of the 
vector fields Q. 

For example, for Q = {31,32}, Lie((?) is given by the linear combinations of 
the following Lie products: 

degree 1: 31,32 

degree 2: [31,32] 

degree 3: [31, [31,32]], [32, [31,52]] 

degree 4: [31, [31, [31,32]], [31, [32, [31,32]]], [32, [31, [31,32]], [32, [32, [31,32]]] 


Since Lie products obey the following identities, 

• \Si,9i] = 0 

• Idiidj] = ~[dj>9i] 

• [30 [ 35 ,3fc]] + \9k, [gi,9j]\ + \9j, [9k, 3i]] = 0 (the Jacobi identity), 
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not all bracket combinations need to be considered at each degree level. 

In practice, there will be a finite degree k beyond which higher-degree 
Lie products yield no more information about the Lie algebra. This hap¬ 
pens, for example, when the dimension of the Lie algebra is n at all g, i.e., 
dim(Lie(C/)(g)) = n for all q; no further Lie brackets can yield new motion 
directions, as all motion directions have already been obtained. If this never 
happens, however, in general there is no way to know when to stop trying higher- 
degree Lie products. For a class of systems (13.25) called regular , however, if 
there is a degree k that yields no new motion directions not included at lower 
degrees, then there is no need to look deeper. 

With all of this as background, we are finally ready to state our main theorem 
on controllability. 

Theorem 13.2. The control system (13.25), with vector fields Q = { g\(q ),..., g m 
is small-time locally accessible from q */dim(Lie(t/)(g)) = n and span(Z7) = R m . 
If additionally pos fU) = R m , then the system is small-time locally controllable 
from q. 

We omit the proof, but the intuition is as follows. If the Lie algebra is full 
rank, then the vector fields (followed both forward and backward) locally permit 
motion in any direction. If pos (Id) = K m (like a car with a reverse gear), then 
it is possible to directly follow all vector fields forward or backward, or it is 
possible to switch between feasible controls to follow any vector field forward 
and backward arbitrarily closely, and therefore the Lie algebra rank condition 
implies STLC. If the controls only satisfy span(Z7) = M. m (like a forward-only 
car), then some vector fields may only be followed forward or backward. Still, 
the Lie algebra rank condition ensures there are no equality constraints on the 
reachable set, so the system is STL A. 

For any system of the form (13.25), the question of whether the velocity 
constraints are integrable is finally answered by Theorem 13.2. If the system is 
STLA at any q , the constraints are not integrable. 

Let us apply Theorem 13.2 to a few examples. 

Example 13.2 (Controllability of the canonical nonholonomic mo¬ 
bile robot). In Example 13.1, we computed the Lie bracket g 3 = [ 51 , 92 ] = 
(0, sintp, — cos <f) T for the canonical nonholonomic robot. Putting the column 
vectors 91 (g), 92 (g), and 93 (g) side-by-side in a matrix and calculating its de¬ 
terminant, we find 


det[ 91 (g) 92 (g) 93 (g) ]=det 


0 10 

cos (j) 0 sin cj> 

sin </> 0 — cos (j) 


= cos 2 </> + sin 2 (f = 1 , 


i.e., the three vector fields are linearly independent at all g, and therefore the 
dimension of the Lie algebra is three at all q. By Theorem 13.2 and the control 
sets illustrated in Figure 13.11, the unicycle, diff-drive, and car are all STLC at 
all g, while the forward-only car is only STLA at all g. Each of the unicycle, 
diff-drive, car, and forward-only car is controllable in the obstacle-free plane. 
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Example 13.3 (Controllability of the full configuration of the unicy¬ 
cle). We already know from the previous example that the unicycle is STLC 
on its (</>, x, y ) subspace; what if we include the rolling angle 6 in the descrip¬ 
tion of the configuration? According to Equation (13.12), for q = (cj),x, y,d ) T , 
the two vector fields are 31 (g) = ( 0 , r cos </>, r sin cj>, 1 ) T and g 2 (g) = ( 1 , 0 , 0 , 0 ) T . 
Calculating the degree 2 and 3 Lie brackets 

53 (<?) = [9i,92]{q) = ( 0 , r sin (/>, -r cos<£, 0 ) T 
54 (g) = [ 52 , 53 ](g) = ( 0 , r cos <j>, r sin <f>,0) T , 

we see that these directions correspond to sideways translation and forward/backward 
motion without changing the wheel rolling angle 5, respectively. These are 
clearly linearly independent of 31 (g) and 32 (g), but we can confirm that by 
evaluating 

det[ 31(g) 32(g) 33(g) 34(g) } = ~r 2 , 

i.e., dim(Lie(f7)(g)) = 4 for all q. Since pos (li) = K 2 for the unicycle by Fig¬ 
ure 13.11, the unicycle is STLC at all points in its four-dimensional configuration 
space. 

Think about a short “parallel parking” type maneuver which results in a 
net change in the rolling angle 6 with zero net change in the other configuration 
variables. 

Example 13.4 (Controllability of the full configuration of the diff 
drive). The full configuration of the diff-drive is five dimensional, q = ( (f >, x, 3 , $l, 0r) t , 
including the angles of both wheels. The two control vector fields are given in 
Equation (13.14). Proceeding to take Lie brackets of these vector fields, we 
would find that we can never create more than four linearly independent vector 
fields, i.e., 

dim(Lie(£)(g)) = 4 

at all q. This is because there is a fixed relationship between the two wheel angles 
( 0 l, 0 r) and the angle of the robot chassis <j). The three velocity constraints 
(dim( 3 ) = 5,dim(u) = 2) implicit in the kinematics (13.14) therefore can be 
viewed as two nonholonomic constraints and one holonomic constraint. On the 
full five-dimensional configuration space, the diff-drive is nowhere STL A. 

Since we usually only worry about the configuration of the chassis, this 
negative result is not of much concern. 

13.3.3 Motion Planning 

13.3.3.1 Obstacle-Free Plane 

It is easy to find feasible motions between any two chassis configurations qo 
and g goa i in the obstacle-free plane for any of the four nonholonomic robot 
models (unicycle, diff-drive, car, or forward-only car). The problem gets more 
interesting when we try to optimize an objective function. Below we consider 
shortest paths for the forward-only car, shortest paths for the car, and fastest 
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Figure 13.15: The two classes of shortest paths for a forward-only car. The 
CSC path could be written RSL, and the CC a C path could be written LR a L. 


paths for the diff-drive. The solutions to these problems depend on optimal 
control theory, and the proofs are left to the original references (see the Notes 
and References at the end of the chapter). 

Shortest Paths for the Forward-Only Car The shortest path problem is 
to find a path from q o to <7 g0 ai that minimizes the length of the path followed by 
the robot’s reference point. This is not an interesting question for the unicycle or 
the diff-drive; a shortest path for each of them is a rotation to point toward the 
goal position (a; goa i, y goa i), a translation, then a rotation to the goal orientation. 
The total path length is y 7 (^o - z g oai) 2 + (yo - 2/goai) 2 - 

The problem is more interesting for the forward-only car, sometimes called 
the Dubins car in honor of the mathematician who first studied the structure of 
the shortest planar curves with bounded curvature between two oriented points. 

Theorem 13.3. For a forward-only car with the control set shown in Fig¬ 
ure 13.11, shortest paths consist only of arcs at the minimum turning radius and 
straight-line segments. Denoting a circular arc segment as C and a straight-line 
segment as S, the shortest path between any two configurations follows either 
the sequence (a) CSC or (b) CC a C, where C a indicates a circular arc of angle 
a > 7T. Any of the C or S segments can be of length zero. 

The two optimal path classes for a forward-only car are illustrated in Fig¬ 
ure 13.15. We can calculate the shortest path by enumerating the possible CSC 
and CC a C paths. Construct the two minimum-turning-radius circles of the ve¬ 
hicle at both qo and q goa i and solve for (a) the points where lines (with the 
correct heading direction) are tangent to one of the circles at qo and one of the 
circles at q goa i, and (b) the points where a minimum-turning-radius circle (with 
the correct heading direction) is tangent to one of the circles at qo and one of the 
circles at g goa i- The solutions to (a) correspond to CSC paths and the solutions 
to (b) correspond to CC a C solutions. The shortest of all the solutions is the 
optimal path. The shortest path may not be unique. 

If we break the C segments into two categories, L (when the steering wheel 
is pegged to the left) and R (when the steering wheel is pegged to the right), 
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Figure 13.16: Three of the nine classes of shortest paths for a car. 


we see there are four types of CSC paths ( LSL , LSR, RSL , and RSR) and 
two types of CC a C paths ( RL a R and LR a L). 

A shortest path is also a minimum-time path for the forward-only car control 
set illustrated in Figure 13.11, where the only controls (v,u) ever used are the 
three controls (^ m ax,0) (an S segment) and (u m ax,±w max ) (a C segment). 

Shortest Paths for the Car The shortest paths for a car with a reverse gear, 
sometimes called the Reeds-Shepp car in honor of the mathematicians who 
first studied the problem, again only use straight-line segments and minimum- 
turning-radius arcs. Using the notation C for a minimum-turning-radius arc, C a 
for an arc of angle a, S for a straight-line segment, and | for a cusp (a reversal 
of the linear velocity), Theorem 13.4 enumerates all the possible shortest path 
sequences. 

Theorem 13.4. For a car with the control set shown in Figure 13.11, the 
shortest path between any two configurations is in one of the following nine 
classes: 

c\c\c cc\c c\cc cc a \c a c c\c a c a \c 

C\C n/2 SC csc n/2 \c C\C„ /2 SC n/2 \C CSC 

Any of the C or S segments can be of length zero. 

Three of the nine shortest path classes are illustrated in Figure 13.16. Again, 
the actual shortest path may be found by enumerating the finite set of possible 
solutions in the path classes in Theorem 13.4. The shortest path may not be 
unique. 

If we break the C segments into four categories, L + , L~ , i? + , and R~, where 
L and R mean the steering wheel is turned all the way to the left or right and 
+ and — indicate the gear shift (forward or reverse), then the nine path classes 
of Theorem 13.4 can be expressed as (6 x 4) + (3 x 8) = 48 different types: 

4 types (6 classes): C\C\C, CC\C, C\CC, CC a \C a C, C\C a C a \C, 
C\C„ /2 SC„ /2 \C 

8 types (3 classes): C\C„ /2 SC, CSC„ /2 \C , CSC 
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motion 

segments 

number 
of types 

motion sequences 

1 

4 

F , B , R, L 

2 

8 

FR, FL , BR, BL , RF, RB , LF, LB 

3 

16 

FRB , FLB , FR n B, FL^B, 

BRF, BLF , BR n F , BL^F, 

RF R, RFL , RBR, RBL , 

LFR , LFL, LBR , LBL 

4 

8 

FRBL, FLBR , BRFL, BLFR, 
RFLB, RBLF, LFRB, LBRF 

5 

4 

FRBLF, FLBRF, BRFLB , BLFRB 


Table 13.1: The 40 time-optimal trajectory types for the diff-drive. The notation 
R v and indicate spins of angle n. 


The four types for six of the path classes are determined by the four different 
initial motion directions, L + , L ~, R + , and R~. The eight types for three of the 
path classes are determined by the four initial motion directions and whether 
the turn is to the left or the right after the straight-line segment. There are only 
four types in the C\C^i^SC^j^\C class because the turn after the S segment is 
always opposite the turn before the S segment. 

If it takes zero time to reverse the linear velocity, a shortest path is also a 
minimum-time path for the car control set illustrated in Figure 13.11, where the 
only controls (v,ui) ever used are the two controls (±w max ,0) (an S segment) or 
the four controls (±w max ,±w max ) (a C segment). 

Minimum-Time Motions for the Diff-Drive For a diff-drive robot with 
the diamond-shaped control set in Figure 13.11, any minimum-time motion 
consists of only translational motions and spins in place. 

Theorem 13.5. For a diff-drive robot with the control set illustrated in Fig¬ 
ure 13.11, minimum-time motions consist of forward and backward translations 
(F and B) at maximum speed ±v max and spins in place (R and L for right turns 
and left turns) at maximum angular speed ±uj max . There are fO types of time- 
optimal motions, categorized in Table 13.1 by the number of motion segments. 
The notations R„ and L_ir indicate spins of angle tt. 

Note that Table 13.1 includes both FR„B and FL^B, which are equivalent, 
as well as BR n F and BL^F. Each of the trajectory types is time optimal 
for some pair {<7o,g goa i}, and the time-optimal trajectory may not be unique. 
Notably absent are three-segment sequences where the first and last motions 
are translations in the same direction (i.e., FRF, FLF , BRB , and BLB). 

While any reconfiguration of the diff-drive can be achieved by spinning, 
translating, and spinning, in some cases other three-segment sequences have a 
shorter travel time. For example consider a diff-drive with u max = w max = 1 
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(1.924, 0.383) 


1 



-7ji/8 



LFR 


FRB 


Figure 13.17: (Top) A motion planning problem specified as a motion from 
qo = (0, 0, 0) to g goa i = (—77 t/ 8, 1.924,0.383). (Bottom left) A non-optimal LFR 
solution taking time 5.103. (Bottom right) The time-optimal FRB solution, 
through a “via point,” taking time 4.749. 

and qo = 0, <7g 0 ai = (—7n/8, 1.924,0.383) as shown in Figure 13.17. The time 
needed for a spin of angle a is |a|/w max = \a\ and the time for a translation of 
d is |d|/u max = |d|. Therefore, the time needed for the LFR sequence is 


^ + 1.962+^ = 5.103 
16 16 


while the time needed for the FRB sequence through a “via point” is 


1 + — + 1 = 4.749. 
8 


13.3.3.2 With Obstacles 

If there are obstacles in the plane, the grid-based motion planning methods 
of Chapter 10.4.2 can be applied to the unicycle, diff-drive, car, or forward- 
only car using discretized versions of the control sets in Figure 13.11. See, 
for example, the discretizations in Figure 10.14 that use the extremal con¬ 
trols from Figure 13.11. Using the extremal controls takes advantage of our 
observation that shortest paths for cars and the diff-drive consist of minimum- 
turning-radius turns and straight-line segments. Also, because the C-space is 
only three-dimensional, the grid size should be manageable for reasonable reso¬ 
lutions along each dimension. 
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Figure 13.18: (Left) The original path from q( 0) to g(l), found by a motion 
planner that does not respect the car’s motion constraints. (Right) The recursive 
path subdivision transformation terminates with via points at g(l/4) and g( 1/2). 


We can also apply the sampling methods of Chapter 10.5. For RRTs, we can 
again use a discretization of the control set, as mentioned above, or, for both 
PRMs and RRTs, the local planner that attempts to connect two configurations 
could use the shortest paths from Theorem 13.3, Theorem 13.4, or Theorem 13.5. 

Another option for a car is to use any efficient obstacle-avoiding path planner, 
even if it ignores the motion constraints of the vehicle. Since the car is STLC, 
and since the free configuration space is defined to be open (obstacles are closed, 
containing their boundaries), the car can follow the path arbitrarily closely. To 
follow the path closely, however, the motion may be slow—imagine using parallel 
parking to travel a kilometer down the road. 

Instead, the initial constraint-free path can be quickly transformed into a 
fast, feasible path that respects the car’s motion constraints. To do this, repre¬ 
sent the initial path as q(s),s £ [0,1]. Then try to connect q( 0) to g(l) using 
a shortest path from Theorem 13.4. If this path is in collision, then divide the 
original path in half and try to connect g(0) to g(l/2) and g(l/2) to g(l) using 
shortest paths. If either of these are in collision, divide that path, and so on. 
Because the car is STLC and the initial path lies in open free space, the process 
will eventually terminate, and the new path consists of a sequence of subpaths 
from Theorem 13.4. The process is illustrated in Figure 13.18. 

13.3.4 Feedback Control 

We can consider three types of feedback control problems for the canonical 
nonholonomic mobile robot (13.18) with controls (v,w): 

(i) Stabilization of a configuration. As we saw in Theorem 13.1, no 
time-invariant feedback law continuous in the state variables can stabilize 
a configuration for a nonholonomic mobile robot. There do exist time- 
varying and discontinuous feedback laws that accomplish the task, but we 
will not consider this problem further in this chapter. 
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(ii) Trajectory tracking. Given a desired trajectory qd{t), drive the error 
q(t) — qd{t ) to zero as time goes to infinity. 

(iii) Path tracking. Given a path q(s), follow the geometric path, without 
regard to the time of the motion. This provides more control freedom than 
the trajectory tracking problem; essentially, we can choose the reference 
configuration along the path to help reduce tracking error, in addition to 
choosing (v,ui). 

Path tracking and trajectory tracking are “easier” than stabilizing a config¬ 
uration, in the sense that there exist continuous time-invariant feedback laws 
to stabilize the desired motions. In this section we consider the problem of 
trajectory tracking. 

Assume that the reference trajectory is specified as qd(t) = (<f>d{t),Xd(t), Ud{t)) 
for t £ [0, T], with a corresponding nominal control (vd(t),u) d (t)) £ int (U) for 
t £ [0, T]. The requirement that the nominal control be in the interior of the 
feasible control set U is to ensure that some control effort is “left over” to ac¬ 
commodate small errors. This implies that the reference trajectory is neither a 
shortest path nor a time-optimal trajectory, since optimal motions saturate the 
controls. The reference trajectory could be planned using not-quite-extremal 
controls. 

A simple first controller idea is to choose a reference point P on the chassis 
of the robot (not on the axis of the two driving wheels), as in Figure 13.12. 
The desired trajectory qd{t) is then represented by the desired trajectory of the 
reference point (xpd(t),ypd(t)). To track this reference point trajectory, we can 
use a proportional feedback controller 


Xp 


kp(x Pd - Xp) 

. yp . 


k p (ypd ~ y P ) 


(13.29) 


where k p > 0. This simple linear control law is guaranteed to pull the ac¬ 
tual point position along with the moving desired point position. The velocity 
(ip,yp) calculated by the control law (13.29) is converted to (v,w) by Equa¬ 
tion (13.21). 

The idea is that, as long as the reference point is moving, over time the 
entire robot chassis will line up with the desired orientation of the chassis. The 
problem is that the controller may choose the opposite orientation of what is 
intended; there is nothing in the control law to prevent this. Figure 13.19 shows 
two simulations, one where the control law (13.29) produces the desired chassis 
motion and one where the control law causes an unintended reversal in the sign 
of the driving velocity v. In both simulations, the controller succeeds in causing 
the reference point to track the desired motion. 

To fix this, let us explicitly incorporate chassis angle error in the control 
law. The fixed space frame is written {s}, the chassis frame {b} is at the point 
between the two wheels of the diff-drive (or the two rear wheels for a car) with 
the forward driving direction along the Xb-axis, and the frame corresponding to 
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Figure 13.19: (Left) A nonholonomic mobile robot with a reference point. (Mid¬ 
dle) A scenario where the linear control law (13.29) tracking a desired reference 
point trajectory yields the desired trajectory tracking behavior for the entire 
chassis. (Right) A scenario where the point-tracking control law causes an un¬ 
intended cusp in the robot motion. The reference point converges to the desired 
path but the robot’s orientation is opposite the intended orientation. 



Figure 13.20: The space frame {s}, the robot frame {b}, and the desired con¬ 
figuration {d} driving forward along the planned path. The heading direction 
error is <j> e = (f> — <f>d- 


qd(t) is written {d}. We define the error coordinates 


4*e 


' 1 0 o' 


1 

1 

i_ 

X e 

= 

0 cos (f>d sin (f>d 


x - Xd 

. y>- 


0 — sin tfid cos 4>d 


. y-yd 


(13.30) 


as illustrated in Figure 13.20. The vector (x e ,y e ) is the {s}-coordinate error 
vector (x — x d , y — yd) expressed in the reference frame {d}. 

Consider the nonlinear feedforward plus feedback control law 


V 


(v d - kl\v d \(x e + J/e tan^e))/ COS (j>e 

UJ 


u>d - ( k 2 v d y e + k 3 \vd\ tan^e) cos 2 4> e 


(13.31) 


where k\,k 2 ,k 3 > 0. Note two things about this control law: (1) if the error 
is zero, the control is simply the nominal control (v d: ujd), and (2) the controls 
grow without bound as <j) e approaches 7r/2 or —7 t/ 2. In practice, we assume 
that \(f> e \ is less than tt/2 during trajectory tracking. 

In the controller for v, the second term, —ki\vd\x e / cos <j> e , attempts to reduce 
x e by driving to catch up or slow down to the reference frame. The third term, 
— ki\vd\y e taj\(f> e /coscj) e , attempts to reduce y e based on the component of the 
forward/backward velocity that impacts y e . 
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< 7 / 0 ) 

Figure 13.21: A mobile robot implementing the nonlinear control law (13.31). 


In the controller for the turning velocity u, the second term, —Zc2^d2/e cos 2 <p e , 
attempts to reduce y e in the future by turning the heading direction of the 
robot toward the reference frame origin. The third term, — k^\vd\ tan <j> e cos 2 (f) e , 
attempts to reduce the heading error <j> e . 

A simulation of the control law is shown in Figure 13.21. 

The control law (13.31) requires \vd\ / 0, so it is not appropriate for stabi¬ 
lizing “spin-in-place” motions for a diff-drive. The proof of the stability of the 
control law requires methods beyond the scope of this book. In practice, the 
gains should be chosen large enough to provide significant corrective action, but 
not so large that the controls chatter at the boundary of the feasible control set 
U. 

13.4 Odometry 

Odometry is the process of estimating the chassis configuration q from wheel 
motions, essentially integrating the wheel velocities. Since wheel rotation sens¬ 
ing is available on all mobile robots, odometry is cheap and convenient. Esti¬ 
mation errors tend to accumulate over time, though, due to unexpected slipping 
and skidding of the wheels and due to numerical integration error. Therefore, it 
is common to supplement odometry with other position sensors, like GPS, visual 
recognition of landmarks, ultrasonic beacons, laser or ultrasonic range sensing, 
etc. Those sensing modalities have their own measurement uncertainty, but er¬ 
rors do not accumulate over time. As a result, odometry generally gives superior 
results on short time scales, but the odometric estimates should either (a) be 
periodically corrected by other sensing modalities or, preferably, (b) integrated 
with other sensing modalities in an estimation framework based on a Kalman 
filter, particle filter, or similar. 

In this section we focus on odometry. We assume each wheel of an omni¬ 
directional robot, and each rear wheel of a diff-drive or car, has an encoder 
that senses how far the wheel has rotated in its driving direction. If the wheels 
are driven by stepper motors, then we know the driving rotation of each wheel 
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based on the steps we have commanded to it. 

The goal is to estimate the new chassis configuration qu+i as a function of 
the previous chassis configuration given the change in wheel angles from the 
instant k to the instant k + 1. 

Let Adi be the change in wheel i' s driving angle since the wheel angle was 
last queried At ago. Since we only know the net change in the wheel driving 
angle, not the time history of how the wheel angle evolved during the time 
interval, the simplest approximation is that the wheel’s angular velocity was 
constant during the time interval, di = Adi/At. The choice of units used to 
measure the time interval is not relevant (since we will eventually integrate the 
chassis body twist Vb over the same time interval), so set At = 1, i.e., di = Ad. 

For omnidirectional mobile robots, the vector of wheel speeds d , and there¬ 
fore Ad, is related to the body twist Vb = (uJb z ,Vbx,Vb y ) T of the chassis by 
Equation (13.8) 

Ad = H(0)V b , 

where H( 0) for the three-omniwheel robot is given by Equation (13.9) and H( 0) 
for the four-mecanum-wheel robot is given by Equation (13.10). Therefore, the 
body twist Vb corresponding to Ad is 

V b = H\0)Ad = FAd, 


where F = H^{ 0) is the pseudoinverse of H{ 0). 


For the three-omniwheel robot, 


V b = FAd = r 


— 1/(3 d) —1/(3 d) —1/(3 d) 

2/3 -1/3 -1/3 

0 —l/(2sin(7r/3)) 1/(2 sin(-7r/3)) 


Ad, (13.32) 


and for the four-mecanum-wheel robot, 


V b = FAd= r - 


— l/((. + w) l/((. + w) \/{(. + w) — 1/(1 + w) 

1111 

- 11-11 


Ad. 


(13.33) 

The relationship Vb = Fd = FAd also holds for the diff-drive robot and the 
car (Figure 13.22), where Ad = (A6*l,A0r) t (the increments of the left and 
right wheels) and 


V b = FAd = r 


-1/(2 d) 
1/2 
0 


l/(2d) 

1/2 

0 



< 


<1 


(13.34) 


Since the wheel speeds are assumed constant during the time interval, so is 
the body twist Vb■ Calling Vb6 the six-dimensional version of the planar twist 
Vb (i.e., Vb6 = (0, 0, uibz, Vbx, Vb y , 0) T ), Vb6 can be integrated to generate the 
displacement created by the wheel angle increment vector Ad: 

T w = el v < 
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wheel 

Figure 13.22: The left and right wheels of a diff-drive or the left and right rear 
wheels of a car. 


wheel 


right 


d 

d 


From T bb i £ SE( 3), which expresses the new chassis frame {t/} relative to the 
initial frame {b}, we can extract the change in coordinates relative to the body 
frame {b}, A q b = (A(j> b , Ax bl Ay b ) T , in terms of (ui bz , v bx , v by ) T : 


if u bz = 0 : A q b 


if uj bz ^ 0 : A q b 


A 4> b 


0 


Ax b 

= 

Vbx 

(13.35) 

Ay b 


. Vb v . 


A 4> b 



u bz 


Ax b 

= 

(■ V bx Sin(uj bz ) + v by ( cos(w fe2 ) - 1 ))/u bz 


A y b 


(v by sin(w te ) +v bx (l- cos (v bz )))/u bz _ 



Transforming A q b in {b} to A q in the fixed frame {s} using the chassis angle 


4>k, 


A q 


1 0 0 

0 cos 4>k — sin <j>k 

0 sin <j>k cos 4>k 


A q b , 


(13.36) 


the updated odometry estimate of the chassis configuration is finally 


qk+i = qk + A q. 

In summary, A q is calculated using Equations (13.35) and (13.36) as a func¬ 
tion of V;, and the previous chassis angle 4>k, and either Equation (13.32), (13.33), 
or (13.34) is used to calculate V b as a function of the wheel angle changes AO for 
the three-omniwheel robot, the four-mecanum-wheel robot, or the diff-drive/car, 
respectively. 


13.5 Mobile Manipulation 

For a robot arm mounted on a mobile base, mobile manipulation is the 
coordination of the motion of the base and the robot joints to achieve a desired 
motion at the end-effector. Typically the motion of the arm can be controlled 
more precisely than the motion of the base, so the most popular type of mobile 
manipulation is to drive the base, park it, let the arm perform the precise motion 
task, then drive away. 
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Figure 13.23: The space frame {s} and the frames {b}, {0}, and {e} attached 
to the mobile manipulator. 


In some cases, however, it is advantageous, or even necessary, for the end- 
effector motion to be achieved by a combination of motion of the base and 
motion of the arm. Defining the fixed space frame {s}, the chassis frame {b}, a 
frame at the base of the arm {0}, and an end-effector frame {e}, the configura¬ 
tion of {e} in {s} is 


X(q,9) = T se (q,9) = T sb (q) T b0 T 0e (9), 


where 9 £ R n is the set of arm joint variables, T$ e (9) is the forward kinematics 
of the arm, T b o is the fixed offset of {0} from {b}, q = (<fi,x,y) is the planar 
configuration of the mobile base, and 


Tsb(q) 


cos <j) 
sin (f> 
0 
0 


— sin (j> 

cos</> 


0 

0 


0 x 
0 y 
1 0 
0 1 


See Figure 13.23. 

Let X(t) be a path for the end-effector as a function of time. Then [V e (t)] = 
-A _1 (f)AT(f) is the se(3) representation of the end-effector twist expressed in {e}. 
Further, let the vector of wheel velocities, whether the robot is omnidirectional 
or nonholonomic, be written u £ R m . For kinematic control of the end-effector 
frame using wheel and joint velocities, we need the Jacobian J e (9) £ R 6x ( m + n ) 
satisfying 


V e = J e (9) 


[ Jbase(0) Jarm(0) ] 


Note that the Jacobian J e (9) does not depend on q ; the end-effector velocity 
expressed in {e} is independent of the configuration of the mobile base. Also, 
we can partition J e (9) into Jbase(^) € R 6xm and J arm (0) £ R 6xn . The term 
Jbase{0)u expresses the contribution of the wheel velocities u to the end-effector’s 
velocity, and the term J aTm (9)9 expresses the contribution of the joint velocities 
to the end-effector’s velocity. 
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In Chapter 5, we developed a method to derive J a rm(9), which is called the 
body Jacobian Jb{9) in that chapter. All that remains is to find Jbase(9). As 
we saw in Section 13.4, for any type of mobile base there exists an F satisfying 


V & = Fu. 

To create a six-dimensional twist V^e corresponding to the planar twist 14, we 
can define the 6 x m matrix 


where two rows of m zeros are stacked above F and one row is placed below. 
Now 

V&6 = FqU. 

This chassis twist can be expressed in the end-effector frame as 


[Adj^^)] 14(3 — [Ad T -l 1 ]^66 — |-A (1 7 ^— 1 1 1 — *A>ase {0)u. 

Therefore, 

•dia.se (11) — 1 (^fT,-) 1 ] . 

Now that we have the complete Jacobian J e (9) = [Jbase(^) >4rm(0)L we can 
perform numerical inverse kinematics (Chapter 6.2) or implement kinematic 
feedback control laws to track a desired end-effector trajectory. For example, 
given a desired end-effector trajectory X d (t), we can choose the kinematic task- 
space feedforward plus feedback control law (11.2), 


V com (t) = V d (t) + K p X err (t), (13.37) 

where 14(i) = X^^X^t) and [A err ] = log(X _1 A4). The commanded end- 
effector-frame twist Vcom(i) is implemented as 


u 

9 


J\{9)V » 


As discussed in Chapter 6.3, it is possible to use a weighted pseudoinverse to 
penalize certain wheel or joint velocities. 

An example is shown in Figure 13.24. The mobile base is a diff-drive and the 
arm moves in the plane with only one revolute joint. The desired motion of the 
end-effector X d (t),t € [0,1], is parameterized by a = —tt t, x e (t) = —3cos(7rf), 
and y e (t) = 3sin(-7rf), where a indicates the planar angle from the x s -axis to the 
x e -axis (see Figure ??). 
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Figure 13.24: A diff-drive with a 1R planar arm with end-effector frame {e}. 
(Top) The desired end-effector trajectory Xd(t) and the initial configuration of 
the robot. (Bottom) Trajectory tracking using the control law (13.37). 

13.6 Summary 

• The chassis configuration of a wheeled mobile robot moving in the plane is 
q = (</>, x , y). The velocity can be represented either as q or as the planar 
twist Vf, = ( uJbz,Vbx,Vb y ) T expressed in the chassis-fixed frame {b}, where 



Ubz 


' 1 

0 

0 



v b = 

b'bx 

= 

0 

COS(/> 

sin t/> 


X 


. Vb V . 


0 

— sin^!> 

cost/) 


. y . 


• The chassis of a nonholonomic mobile robot is subject to a single non- 
integrable Pfaffian velocity constraint A(q)q = [ 0, sin t/>, — cos t/> ] q = 
xsin t/> — y cos</> = 0. An omnidirectional robot, employing omniwheels or 
mecanum wheels, has no such constraint. 

• For a properly constructed omnidirectional robot with m > 3 wheels, there 
































13.6. Summary 


489 


exists a rank 3 matrix H((f)) £ R mx3 that maps the chassis velocity q to 
the wheel driving velocities u: 

u = H(<p)q. 


In terms of the body twist 14, 


u = H{ 0)V b . 

The driving speed limits of each wheel place two parallel planar constraints 
on the feasible body twists, creating a polyhedron V of feasible body 
twists. 

• Motion planning and feedback control for omnidirectional robots is sim¬ 
plified by the fact that there are no chassis velocity equality constraints. 

• Nonholonomic mobile robots are described as driftless linear-in-the-control 
systems 

q = G(q)u, u£U Ci” 1 , 

where G(q) £ R nxm ,n > m. The columns gi(q) of G(q) are called the 
control vector fields. 


• The canonical simplified nonholonomic mobile robot model is 


q = 


x 

y 


= G{q)u 


0 1 

cos (j) 0 
sin (j) 0 


The control sets U differ for the unicycle, diff-drive, car, and forward-only 
car. 


• A control system is small-time locally accessible (STLA) from q if, for any 
time T > 0 and any neighborhood W (q), the reachable set in time less than 
T without leaving W (q) is a full-dimensional subset of the configuration 
space. A control system is small-time locally controllable (STLC) from 
q if, for any time T > 0 and any neighborhood W(q), the reachable set 
in time less than T without leaving W(q) is a neighborhood of q. If the 
system is STLC from a given q, it can locally maneuver in any direction. 

• The Lie bracket of two vector fields gi and <72 is the vector field 

r 1 / dg 2 dgi 

[51 ’ 52] - v V* ■ V 72 


• A Lie product of degree k is a Lie bracket term where the original vector 
fields appear k times. A Lie product of degree 1 is just one of the original 
vector fields. 








490 


Wheeled Mobile Robots 


• The Lie algebra of a set of vector fields Q = {<q,... ,g m }, written Lie(£7), 
is the linear span of all Lie products of degree 1... oo of the vector fields 

G- 

• A driftless control-affine system is small-time locally accessible from q if 
dim(Lie(£/)(g)) = n and span(ZJ) = K m . If additionally pos (U) = R m , 
then the system is small-time locally controllable from q. 

• For a forward-only car in the obstacle-free plane, shortest paths always 
follow a turn at the tightest turning radius ( C ) or straight-line motions 
(S). There are two classes of shortest paths: CSC and CC a C , where C a 
is a turn of angle |a| > ir. Any of the C or S segments can be of length 
zero. 

• For a car with a reverse gear, shortest paths always consist of a sequence 
of straight-line segments or turns at the tightest turning radius. Shortest 
paths always belong to one of nine classes. 

• For the diff-drive, minimum-time motions always consist of turn-in-place 
motions and straight-line motions. 

• For the canonical nonholonomic robot, there is no time-invariant control 
law continuous in the configuration that stabilizes the origin configuration. 
Continuous time-invariant control laws exist to stabilize a trajectory, how¬ 
ever. 

• Odometry is the process of estimating the chassis configuration based 
on how far the robot’s wheels have rotated in their driving direction, 
assuming no skidding in the driving direction and, for typical wheels (not 
omniwheels or mecanum wheels), no slip in the orthogonal direction. 

• For a mobile manipulator with m wheels and n joints in the robot arm, 
the end-effector twist V e in the end-effector frame {e} is written 

Ve = J e (0) 

The 6 x m Jacobian Jbase(^) maps the wheel velocities u to a velocity at 
the end-effector, and the 6 x n Jacobian is the body Jacobian derived in 
Chapter 5. The Jacobian Jbase(^) is given by 

dba.se (d) — I Ad-T" 1 (0) T “ 1 ] -^6 

where F§ is the transformation from the wheel velocities to the chassis 
twist, V{,6 = Fu. 


= [Jbase(0) JarmW] 


13.7 Notes and References 
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Appendix A 

Summary of Useful 
Formulas 


Chapter 2 

• dof = (sum of freedoms of bodies) — (number of independent configuration 
constraints) 

• Griibler’s formula is an expression of the previous formula for mechanisms 
with N links (including ground) and J joints, where joint i has ft de¬ 
grees of freedom and m = 3 for planar mechanisms or m = 6 for spatial 
mechanisms: 

j 

dof = m(JV-l- 

i=l 

• Pfaffian velocity constraints take the form A{0)9 = 0. 

Chapter 3 


Rotations Rigid-Body Motions 


R 6 50(3) : 3 x 3 matrices satisfying T £ SE( 3 

R T R = 7, det R = 1 T = 

where R 

: 4 x 4 matrices 

R p 

0 1 J ’ 

£ 50(3),p e M 3 

R- 1 = R t T~ l = 

R T —R T p 

0 1 



change of coord frame: change of coord frame: 

RabRbc — Ract RabPb Pa 7 n h 1 be. , TabPb — Pa 


continued... 
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Rotations (cont.) 


Rigid-Body Motions (cont.) 


rotating a frame {b}: 

R = Rot(u), 0) 

R s b' = RR s b : rotate 0 about ui s = Cj 
R s b" = R sb R : rotate 6 about uj b = Cj 


T = 


displacing a frame {b 
Rot(w,0) p 

0 1 

T s b' = TT s b'. rotate 6 about Ci s = Cj 
(moves {b} origin), translate p in {s} 
T s b" = T s bT : translate p in {b}, 
rotate 9 about Cj in new body frame 


unit rotation axis is Cj £ 
where llwll = 1 


“unit” screw axis is S = 


ui 

V 


where either (i) ||w|| = 1 or 
(ii) ui = 0 and ||uj| = 1 


for a screw axis {q, s, h} with finite h , 
5 = 


UJ 


s 

V 


—s x q + hs 


angular velocity can be written ui = Cj9 


twist can be written V = SO 


for any 3-vector, e.g., ui £ 


for V = 


u> 

v 



0 

-W 3 

UI 2 



r r i 1 

Ul] = 

UJ3 

0 

-Wl 

£ so( 3) 

[V] = 

[ui\ V 

0 0 


—0J2 

Wi 

0 





identities: for ui,x £ R J ,R £ 50(3): 

,1 T 


M = -[Wj 
M [x} = ([xM) T ,R[ui}R T = [Rui] 


[U!\X = — [X\U!, 
\T nr, ,1 tdT 


£ se( 3 ) 


(the pair (w, v) can be a twist V 
or a “unit” screw axis S , 
depending on the context) 


RR 1 = [w s ], R 1 R = [ojf,] 


TT~ X = [V a ], T~ 1 T=[V b } 


[Ady] — 


x6 


R 0 
[ P ]R R 

identities: [Adr] 1 = [Ad^-i] 
[Ad Tl ][Ad T2 ] = [Ad Tl r 2 ] 


change of coord frame: 
Cla = RabClbi JJa = Rab^b 


change of coord frame: 

S a = [AdT a J<S6, V a = [Adj^JVb 


uiO £ K 3 are exp coords for R £ SO(3) S9 £ R 6 are exp coords for T £ SE(3) 


exp : [Cj\9 £ so(3) —>■ R £ 50(3) 
R = Rot(a), 0) =eW 8 = 

I + sin0[w] + (1 — cos0)[cD] 2 


exp : [5]0 e se(3) ->Te SE{3) 

e [*\o * 


T = e^ e = 


0 


1 


where * = 

(19 + (1 — cos0)[w] + (9 — sin0)[w] 2 )u 


log : R £ 50(3) —> [Cj]6 £ so( 3) 
algorithm in Chapter 3.2.3.3 

log : T £ SE( 3) [5]0 e se(3) 

algorithm in Chapter 3.3.3.2 

moment change of coord frame: 
m a = Rabm b 

wrench change of coord frame: 

?a = (■ m a ,f a ) = [Ad Tb J T T b 
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Chapter 4 

• The product of exponentials formula for a serial chain manipulator is 

space frame: T = e^ 1 ^ 1 ... e^ Sri ^ 6n M 
body frame: T = Me [Bl]ei ... e [Bn]K 

where M is the frame of the end-effector in the space frame when the 
manipulator is at its home position, Si is the spatial twist when joint i 
rotates (or translates) at unit speed while all other joints are at their zero 
position, and Bi is the body twist of the end-effector frame when joint i 
moves at unit speed and all other joints are at their zero position. 

Chapter 5 

• For a manipulator end-effector configuration written in coordinates x, the 
forward kinematics is x = f[G), and the differential kinematics is given by 
x = ^9 = J(9)8, where J{8) is the manipulator Jacobian. 

• Written using twists, the relation is V* = J*(Q)6, where * is either s (space 
Jacobian) or b (body Jacobian). The columns V S i of the space Jacobian 
are 

V S i(0) = [Ad e [ Sl]ei giSj-pSj-JiSi 

and the columns V b i of the body Jacobian are 

V bi (0) = [Ad e - [ B„ 1 s n .., e -[B<+ 1 ]e« +1 ]B»- 

The spatial twist caused by joint i motion is only altered by the config¬ 
urations of joints inboard from joint i (between the joint and the space 
frame), while the body twist caused by joint i is only altered by the con¬ 
figurations of joints outboard from joint i (between the joint and the body 
frame). 

The two Jacobians are related by 

Me) = [Ad Tbs{e) ]Js(e) , j 8 (o) = [Ad Tsb{e) \J b {e). 

• Generalized forces at the joints r are related to wrenches expressed in the 
space or end-effector body frame by 

T= JJ(0)J-*, 

where * is s (space frame) or b (body frame). 

• The manipulability ellipsoid is defined by 

V T (JJ T )~ 1 V= 1, 

where V may be a set of task space coordinate velocities <j, a spatial or 
body twist, or the angular or linear components of a twist, and J is the 
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appropriate Jacobian satisfying V = J{9)9. The principal axes of the 
manipulability ellipsoid are aligned with the eigenvectors of JJ T , and the 
semi-axis lengths are the square roots of the corresponding eigenvalues. 

• The force ellipsoid is defined by 

T t JJ t T = 1, 

where J is a Jacobian (possibly in terms of a minimum set of task space 
coordinates, or in terms of the spatial or body wrench) and T is an end- 
effector force or wrench satisfying r = J T T. The principal axes of the 
manipulability ellipsoid are aligned with the eigenvectors of (JJ T ) _1 , and 
the semi-axis lengths are the square roots of the corresponding eigenvalues. 

Chapter 6 

• The law of cosines states that c 2 = a 2 + b 2 — 2 ab cos 7 , where a, b, and c are 
the lengths of the sides of a triangle and 7 is the interior angle opposite 
side c. This formula is often useful to solve inverse kinematics problems. 

• Numerical methods are used to solve the inverse kinematics for systems 

for which closed-form solutions do not exist. A Newton-Raphson method 
using the Jacobian pseudoinverse is outlined below. 

(i) Initialization: Given T sd and an initial guess do € 1". Set i = 0. 

(ii) Set [V;,] = log (T^ 1 (6i)T sd ). While ||w b || > e u or \\v b \\ > e v for small 

€uj 1 • 

— Set = 0i + </ b (0j)V b . 

— Increment i. 

If J is square and full rank, then Jt = J -1 . If J G l m x" is full rank (rank 
m for n > m or rank n for n < m), i.e., the robot is not at a singularity, 
the pseudoinverse can be calculated as 

= J T (J J T )~ X if J is fat, n > m (called a right inverse , since JJ^ = I) 

J * = (J T J ) _1 J T if J is tall, n <m (called a left inverse , since J = I). 

Chapter 8 

• The Lagrangian is the kinetic minus the potential energy, £(0, 9) = /C(0, &)— 

no)- 

• The Euler-Lagrange equations are 

_ d^dC _ dC 
dt d6 d0 


The equations of motion of a robot can be written in the following equiv¬ 
alent forms: 


r = M(9)9 + h(9,9) 

= M(9)9 + c(9,9)+g(9) 

= M(9)9 + 6 t T (9)9 + g(9) 

= M(9)9 + C(9,9)9 + g(9), 

where M(9) is the nxn symmetric positive-definite mass matrix, h(9,6) is 
the sum of generalized forces due to gravity and quadratic velocity terms, 
c(9,9) are quadratic velocity forces, g(9) are gravitational forces, T(0) is 
an n x n x n matrix of Christoffel symbols of the first kind obtained from 
partial derivatives of M(9) with respect to 9, and 0(9,9) is the nxn 
Coriolis matrix whose (i,j) entry is given by 

n 

c ij (9,9) = ^T ijk (9)9 k . 

fc=l 

If the end-effector of the robot is applying a wrench .7-tip to the environ¬ 
ment, the term J T (9)Ft ; p should be added to the right-hand side of the 
robot’s dynamic equations. 


The symmetric positive-definite rotational inertia matrix of a rigid body 
is 


Th = 


T 7 

-L'cr.cr. 


j 


Lsyz 


where 


Ixx = S B (y 2 + z 2 )p(x , y, z)dV l yy = f B (x 2 + z 2 )p(x, y, z)dV 
Izz = f B (x 2 + y 2 )p(x,y,z)dV l xy = - J B xyp(x,y, z)dV 
%xz =~ S B xzp(x , y, z)d,V X yz = - f B yzp(x , y, z)dV, 


B is the body, dV is a differential volume element, and p(x 7 y,z ) is the 
density function. 


If Tb is defined in a frame {b} at the center of mass, with axes aligned 
with the principal axes of inertia, then Ib is diagonal. 


If {b} is at the center of mass but its axes are not aligned with the principal 
axes of inertia, there always exists a rotated frame {c} defined by the 
rotation matrix Rb c such that I c = R^ c IbRbc is diagonal. 

If lb is defined in a frame {b} at the center of mass, then I q , the inertia 
in a frame {q} aligned with {b}, but displaced from the origin of {b} by 
g£l 3 in {b} coordinates, is 

I q =X b + m (q T qh - qq T ) 


by Steiner’s theorem. 
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• The spatial inertia matrix Gb expressed in a frame {b} at the center of 
mass is defined as the 6x6 matrix 


Sb 


l b o 

0 ml 


• The Lie bracket of two twists Vi and V 2 is 


where 


[Vi,V 2 ] =ad Vl (V 2 ) = [ad Vl ]V 2) 


[ad v ] = 


M 0 
M M 


G K. 


6x6 


• The twist-wrench formulation of the rigid-body dynamics of a single rigid 
body is 

lb = GbV b - [ad Vi ] T g b V b . 

• The kinetic energy of a rigid body is i GbVb, and the kinetic energy of 
an open-chain robot is \6 T M(6)6. 

• The forward-backward Newton-Euler inverse dynamics algorithm is the 
following: 

Initialization: Attach a frame {0} to the base, frames {1} to {n} to 
the centers of mass of links { 1 } to {n}, and a frame {n + 1 } at the end- 
effector, fixed in the frame {n}. Define to be the configuration of 

{*} in {* — 1 } when 0 7 = 0. Let A be the screw axis of joint i expressed 
in {*}, and G% be the 6 x 6 spatial inertia matrix of link i. Define Vo to be 
the twist of the base frame {0} expressed in base frame coordinates. (This 
quantity is typically zero.) Let g 6 R 3 be the gravity vector expressed in 
base frame {0} coordinates, and define Vo = (0, —g). (Gravity is treated 
as an acceleration of the base in the opposite direction.) Define l n +1 = 
l t i p = (rn t ip,/tip) to be the wrench applied applied to the environment 
by the end-effector expressed in the end-effector frame {n + 1 }. 

Forward iterations: Given 0 , 9,6 1 for i = 1 to n do 

Ti-i,i = 

V 7 = Adiyj-i (Vi-i) + AiQi 

V* = AdT^Vi—O + lVq-A^ + A^ 


Backward iterations: For i = n to 1 do 

li = Ady. +l . (A+i) + GiVi — ady.(^iVj) 

n = ij Ai. 
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• Let Jib(0) to be the Jacobian relating 9 to the body twist Vi in link i’s 
center-of-mass frame {i}. Then the mass matrix M(9) of the manipulator 
can be expressed as 


M{6) = Y J &)GiM0). 

• The robot’s dynamics M(0)9 + h(6, 6) can be expressed in the task space 
as 

F = A (0)V + V (9,V), 

where T is the wrench applied to the end-effector, V is the twist of the end- 
effector, and _F, V, and the Jacobian J{9) are all defined in the same frame. 
The task-space mass matrix A (6) and gravity and quadratic velocity forces 
77 ( 0 , V) are 

A(0) = J~ T M(9)J~ 1 
rj{9,V) = J" T /i(0, J~ X V)~ A(0)jj" 1 V. 

• Define two n x n projection matrices of rank n — k 

P{0) = I — A T (AM~ 1 A t )~ 1 AM~ 1 
P§{9) = M~ X PM = I - M- x A t {AM~ x A t )- 1 A 

corresponding to the k Pfaffian constraints acting on the robot, A(9)9 = 0, 
A £ R kxn . Then the n + k constrained equations of motion 

r = M(9)9 + h(9,9) + A T (9) A 

,4(0)0 = 0 

can be reduced to these equivalent forms by eliminating the Lagrange 
multipliers A: 

Pt = P(M9 + h) 

Pgd = P S M-\t-K). 

The matrix P projects away joint force/torque components that act on the 
constraints without doing work on the robot, and the matrix Pg projects 
away acceleration components that do not satisfy the constraints. 

Chapter 9 

• A straight-line path in joint space is given by 0(s) = 0 s tart + s(0end — 0 s tart) 
as s goes from 0 to 1. 

• A constant-screw-axis motion of the end-effector from X sta rt £ SE( 3) to 
X end is X(s) = X start exp(log(Jf s ^ rt Jf e nd)s) as s goes from 0 to 1. 
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• The path-constrained dynamics of a robot can be written 
m(s)s + c(s)s 2 + g(s) = r £ K" 
as s goes from 0 to 1. 


Appendix B 


Other Representations of 
Rotations 

B.l Euler Angles 

As we established earlier, the orientation of a rigid body can be parametrized 
by three independent coordinates. For example, consider a rigid body with a 
body frame {b} attached to it, initially aligned with the space frame {s}. Now 
rotate the body by a about the body Zb-axis, then by /3 about the body y b -axis, 
and finally by 7 about the body x b -axis. Then (a, /3, 7 ) are the ZYX Euler 
angles representing the final orientation of the body (see Figure B.l). Since the 
successive rotations are done in the body frame, this corresponds to the final 
rotation matrix 


R(a, /3, 7 ) = I Rot(z, a) Rot(y, fi) Rot(x, 7 ), 


where 


Rot(z, a) 


cos a 

— sin a 

0 ■ 


cos/3 

0 

sin/3 

sin a 

cos a 

0 

, Rot(y,/3) = 

0 

1 

0 

0 

0 

1 


— sin/3 

0 

cos/3 


Rot(x, 7 ) 


1 0 0 

0 cos 7 — sin 7 
0 sin 7 cos 7 


Writing out the entries explicitly, we get 


-R(«,/3,7) = 


Cq-C^ 

CqjS^S^ — SojC-y 

CojS^C-y H - S a S 7 


s a s / 3 s 7 c Q; c 7 

Sq;S^C 7 C q S 7 

-S/3 

C^S-y 

C/3C 7 

sin a, 

z a for cos a, etc. 



(B.l) 
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yb 


Figure B.l: To understand the ZYX Euler angles, use the corner of a box or a 
book as the body frame. The ZYX Euler angles are successive rotations of the 
body about the Zb-axis by a, the y b -axis by /3, and the Xb-axis by 7 . 

We now ask the following question: given an arbitrary rotation matrix R , 
does there exist satisfying Equation (B.l)? In other words, can the 

ZYX Euler angles represent all orientations? The answer is yes, and we prove 
this fact constructively as follows. Let rij be the ij- th element of R. Then 
from Equation (B.l) we know that ’’ll + *21 = cos 2 /3; as long as cos /3 ^ 0, or 
equivalently /3 ^ ±90°, we have two possible solutions for /?: 



and 



(The atan2 two-argument arctangent is described at the beginning of Chapter 6 .) 
In the first case /3 lies in the range [—90°,90°], while in the second case /? lies 
in the range [90°, 270°]. Assuming the /3 obtained above is not ±90°, a and 7 
can then be determined from the following relations: 


a = atan 2 (f 2 i, Th) 
7 = atan 2 (r 3 2 , r 33 ). 


In the event that /? = ±90°, there exists a one-parameter family of solutions 
for a and 7 . This is most easily seen from Figure B.3. If j3 = 90°, then a and 
7 represent rotations (in the opposite direction) about the same vertical axis. 
Hence, if (a, ,$, 7 ) = (d,90°,7) is a solution for a given rotation R , then any 
triple ( a ', 90°, 7 ') where a' — 7 ' = a — 7 is also a solution. 







B.l. Euler Angles 
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B.1.1 Algorithm for Computing the ZYX Euler Angles 


that satisfy 


R = 


wish to find angles a 

7 € [0, 27t) and (3 £ 

—tt/2, tt/2) 


So;C"-y 

CQ-S^C-y ~\~ SojS/y 




So-S^S/y — I - 

SojS^Cy CojS-y 


(B.2) 

-S/3 

C^S-y 

CyS C<y 




where s a is shorthand for sin a, c a for cos a, etc. Denote by r,; 7 - the ij -th entry 
of R. 

(i) If r 3 1 ± ±1, set 


P 

= atan 2 ^-r 3i , yjr\ x + r^J 

(B.3) 

a 

= atan 2 (r 2 i,rn) 

(B.4) 

7 

= atan 2 (r 32 ,r 33 ), 

(B.5) 


where the square root is taken to be positive. 

(ii) If r 3 1 = 1, then f3 = 7 r/ 2 , and a one-parameter family of solutions for a 
and 7 exists. One possible solution is a = 0 and 7 = atan 2 (ri 2 , T 22 )- 

(iii) If r 3 i = —1, then (3 = — 7 r/2, and a one-parameter family of solutions for 
a and 7 exists. One possible solution is a = 0 and 7 = —atan 2 (ri 2 , ^ 22 ). 

B.1.2 Other Euler Angle Representations 

The ZYX Euler angles can be visualized using the wrist mechanism shown in 
Figure B.2. The ZYX Euler angles (a,/3, 7 ) refer to the angle of rotation about 
the three joint axes of this mechanism. In the figure the wrist mechanism is 
shown in its zero position, i.e., when all three joints are set to zero. 

Four reference frames are defined as follows: frame {0} is the fixed frame, 
while frames {1}, {2}, and {3} are attached to the three links of the wrist 
mechanism as shown. When the wrist is in the zero position, all four refer¬ 
ence frames have the same orientation. At the joint angles (a,/3, r y), frame {1} 
relative to {0} is Roi(a) = Rot(z, a), and similarly Ri 2 (/?) = Rot(y,/3) and 
^ 23 ( 7 ) = Rot(x, 7 ). Therefore Ro^{a , /?, 7 ) = Rot(z, a) Rot(y, /3) Rot(x, 7 ) as in 
Equation (B.l). 

It should be evident that the choice of zero position for (3 is, in some sense, 
arbitrary. That is, we could just as easily have defined the home position of the 
wrist mechanism to be as in Figure B.3; this would then lead to another three- 
parameter representation (a, (3, 7 ) for 50(3). In fact, Figure B.3 illustrates the 
ZYZ Euler angles. The resulting rotation matrix can be obtained via the 
following sequence of rotations, equivalent to rotating the body in Figure B.l 
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Figure B.2: Wrist mechanism illustrating the ZYX Euler angles. 


first about the body’s Zb-axis, then about the y b -axis, then about the Zb-axis: 


R{<x,P, 7 ) 


Rot(z, a)Rot(y, /3)Rot(z, 7 ) 


C/3 

0 

S/3 

0 

1 

0 

-S/3 

0 

C/3 


S a S 7 

SckC^C^ C a S 7 
S/3C 7 


Sq;C 7 

SfyC^S 7 Cq;C 7 

S/3S 7 


c 7 —s 7 0 

s 7 c 7 0 

0 0 1 

C a S/3 
S a S/3 
c /3 . 


(B. 6 ) 


Just as before, we can show that for every rotation R S SO(3), there exists 
a triple (a,/3, 7 ) that satisfies i? = R(a,/3,j ) for R(a,/3,j) as given in Equa¬ 
tion (B. 6 ). (Of course, the resulting formulas will differ from those for the ZYX 
Euler angles.) 

From the wrist mechanism interpretation of the ZYX and ZYZ Euler angles, 
it should be evident that for Euler angle parametrizations of SO( 3), what really 
matters is that rotation axis 1 is orthogonal to rotation axis 2 , and that rotation 
axis 2 is orthogonal to rotation axis 3 (axis 1 and axis 3 need not necessarily be 
orthogonal to each other). Specifically, any sequence of rotations of the form 


Rot(axisl, a) ■ Rot(axis2, /3) ■ Rot(axis3, 7 ), (B-7) 


where axisl is orthogonal to axis2, and axis2 is orthogonal to axis3, can serve 
as a valid three-parameter representation for SO( 3). The angle of rotation for 
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Figure B.3: Configuration corresponding to /? 


90° for ZYX Euler angles. 


the first and third rotations ranges in value over a 2n interval, while that of the 
second rotation ranges in value over an interval of length n. 


B.2 Roll-Pitch-Yaw Angles 

While Euler angles refer to a sequence of rotations about a body-fixed frame, the 
roll-pitch-yaw angles refer to a sequence of rotations about axes fixed in the 
space frame. Referring to Figure B.4, given a frame at the identity configuration 
(that is, R = /), we first rotate this frame by an angle 7 about the x-axis of 
the fixed frame, followed by an angle /3 about the y-axis of the fixed frame, and 
finally by an angle a about the z-axis of the fixed frame. 

Since the three rotations are in the fixed frame, the final orientation is 


R(a,/3,7) 


Rot(z, a)Rot(y, /?)Rot(x, 7 )/ 



Sq! 

0 ' 


C /3 

0 S/3 


' 1 

0 

0 

Sq! 


0 


0 

1 0 


0 

c 7 

—s 7 

0 

0 

1 


. -S/3 

1 

03. 

O 

O 


0 

s 7 


Cq;C [3 

Cq 


- 

Sq: C 7 

HI - S 

aS 7 




Sqi 

S^S 7 + 

Cq;C 7 


c 

a^ 7 



-S/3 


C/3S 7 







I 

(B. 8 ) 


This product of three rotations is exactly the same as that for the ZYX Euler 
angles given in (B.2). We see that the same product of three rotations admits 
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Z 


Z 



z 


Y 


Figure B.4: Illustration of XYZ roll-pitch-yaw angles. 


two different physical interpretations: as a sequence of rotations with respect 
to the body frame (ZYX Euler angles), or, reversing the order of rotations, as 
a sequence of rotations with respect to the fixed frame (the XYZ roll-pitch-yaw 
angles). 

The terms roll, pitch, and yaw are often used to describe the rotational mo¬ 
tion of a ship or aircraft. In the case of a typical fixed-wing aircraft, for example, 
suppose a body frame is attached such that the x-axis is in the direction of for¬ 
ward motion, the z-axis is the vertical axis pointing downward toward ground 
(assuming the aircraft is flying level with respect to ground), and the y-axis 
extends in the direction of the wing. The roll, pitch, and yaw motions are then 
defined according to the XYZ roll-pitch-yaw angles (a,/?, 7 ) of Equation (B. 8 ). 


B.3 Unit Quaternions 


One disadvantage of the exponential coordinates on 50(3) is that because of 
the division by sin 9 in the logarithm formula, the logarithm can be numerically 
sensitive to small rotation angles 6. The necessary singularity of the three- 
parameter representation occurs at R = I. The unit quaternions are an 
alternative representation of rotations that alleviates this singularity, but at 
the cost of introducing an additional fourth parameter. We now illustrate the 
definition and use of these coordinates. 

Let R £ 50(3) have the exponential coordinate representation Cj6 1 i.e., R = 
eM e ; where as usual ||w|| = 1 and 9 £ [0, 7 r]. The unit quaternion representation 
of R is constructed as follows. Define get 4 according to 




(B.9) 


q as defined clearly satifies ||g|| = 1. Geometrically, q is a point lying on the 
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three-dimensional unit sphere in R 4 , and for this reason the unit quaternions 
are also identified with the three-sphere, denoted S 3 . Naturally among the 
four coordinates of q , only three can be chosen independently. Recalling that 

1 + 2cos0 = tr R, and using the cosine double angle formula, i.e., cos 2 <j> = 

2 cos 2 0—1, the elements of q can be obtained directly from the entries of R as 
follows: 


9o = -Vl + rn + r 22 +r 33 


gi 

Q2 

Q3 


1 

4g 0 


T32 - r 2 3 

n 3 - r 3 1 
r 2i ~ 2 12 


(B.10) 

(B.ll) 


Going the other way, given a unit quaternion (qo, gi, g 2 , g 3 ), the correspond¬ 
ing rotation matrix R is obtained as a rotation about the unit axis in the direc¬ 
tion of (gi, g 2 , ga), by an angle 2 cos -1 go- Explicitly, 


R = 


% + gi - g 2 - g 3 
2(gog3 + gig 2 ) 
2(gig3 - gog2) 


2(gig 2 - g 0 g 3 ) 

go - g? + gl - gf 

2(gogi + g 2 g 3 ) 


2(gog2 + gigs) 
2(g2g3 - gogi) 

go - g? - gi + g 3 


(B.12) 


From the above explicit formula it should be apparent that both q € S 3 and its 
antipodal point — g € S 3 produce the same rotation matrix R. For every rotation 
matrix there exists two unit quaternion representations that are antipodal to 
each other. 

The final property of the unit quaternions concerns the product of two rota¬ 
tions. Let R q , Rp £ SO(3) denote two rotation matrices, with unit quaternion 
representations ±g, ±p € S 3 , respectively. The unit quaternion representation 
for the product R q R p can then be obtained by first arranging the elements of q 
and p in the form of the following 2x2 complex matrices: 


go + % 

g2 + ip3 

, P = 

Po + ipi 

P2 + iP3 

-g 2 + *g 3 

O 

1 

<s> 


-P2 + IP3 

Po - ipi 


(B.13) 


where i denotes the imaginary unit. Now take the product N = QP, where the 
entries of N are written 


N = 


n o + ini n 2 + in 3 
—n 2 + m 3 no — ini 


(B.14) 


The unit quaternion for the product R q R p is then given by ±(no, n\, n 2 , n 3 ) 
obtained from the entries of N: 


n 0 


gopo - gipi - q 2 P 2 - g 3 p 3 

ni 


qopi + Poqi + q2P3 - g 3 ^2 

n 2 


qoP2 + poq2 - q\P3 + g 3 pi 

. n 3 . 


. qoP3 + Poq3 + q\P2 - q2Pi 


(B.15) 
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B.4 Cayley-Rodrigues Parameters 

Another set of widely used local coordinates for SO(3) is the Cayley-Rodrigues 
parameters. These parameters can be obtained from the exponential represen¬ 
tation on 50(3) as follows: given R = e^ e for some unit vector uj and angle 9, 
the Cayley-Rodrigues parameter r € K 3 is obtained by setting 


r 


= Cj tan -. 
2 


(B.16) 


Referring again to the radius 7r solid ball picture of 50(3), the above parametriza- 
tion has the effect of infinitely “stretching” the radius of this ball via the tangent 
half-angle function. These parameters can be derived from a general formula at¬ 
tributed to Cayley that is also valid for rotation matrices of arbitrary dimension: 
if R £ 50(3) such that tr(i?) ^ —1, then (/ — R)(I + i?) -1 is skew-symmetric. 
Denoting this skew-symmetric matrix by [r], it is known that R and [r] are 
related as follows: 

R = (/-[r])(/+[r])- 1 (B.17) 

[r] = (I - R)(I + R)~ 1 . (B.18) 


The above two formulas establish a one-to-one correspondence between so(3) 
and those elements of 50(3) with trace not equal to —1. In the event that 
tr(i?) = —1, the following alternative formulas can be used to relate 50(3) 
(this time excluding those with unit trace) and so(3) in a one-to-one fashion: 


R = -(/^[r])(/+[r])- 1 

[r] = (I + R^I-R)- 1 

Equation (B.18) can furthermore be explicitly computed as 

(1 — r T r)I + 2 rr T + 2 [r] 

R = -:- T' - 

1 + r 1 r 

with its inverse mapping given by 


R-R t 
1 +tv{R)' 


(B.19) 

(B.20) 


(B.21) 


(B.22) 


(This formula is valid when tr(I?) ^ —1). The vector r = 0 therefore corre¬ 
sponds to the identity matrix, and — r represents the inverse of the rotation 
corresponding to r. 

The following two identities also follow from the above formulas: 


1 + tr(R) 
R-R t 


4 

1 + r T r 
1 + r T r 


(B.23) 


(B.24) 






BA. Cayley-Rodrigues Parameters 


509 


One of the attractive features of the Cayley-Rodrigues parameters is the 
particularly simple form for the composition of two rotation matrices. If r± and 
r 2 denote the Cayley-Rodrigues parameter representations of two rotations R± 
and respectively, then the Cayley-Rodrigues parameter representation for 
f? 3 = R 1 R 2 , denoted r 3 , is given by 




ri+r 2 + (r 1 x r 2 ) 
1 - rJr-2 


(B.25) 


I 11 the event that rjr 2 = 1, or equivalently tr(RiR 2 ) 
alternative composition formula can be used. Define 

r 

Vl + r T r 

so that the rotation corresponding to r can be written 


— 1, the following 


(B.26) 


R = I + 2\J 1 — s T s [s]+2[s] 2 . (B.27) 

The direction of s coincides with that of r, and ||s|| = sin |. The composition 
law now becomes 


S3 = si 



(B.28) 


Angular velocities and accelerations also admit a simple form in terms of the 
Cayley-Rodrigues parameters. If r(t) denotes the Cayley-Rodrigues parameter 
representation of the orientation trajectory R(t), then in vector form 


- l + ||r||+ X ’' + r) 

(B.29) 

2 ... 
w b = I, ,12 ( r x r + r . 

1 + ||r|| 2 

(B.30) 


The angular acceleration with respect to the inertial and body-fixed frames can 
now be obtained by time-differentiating the above expressions: 


w s = 


iO b = 


1 + IMI 2 

2 

1 + IMI 2 


(r x f + f-r T rw s ) 
(-r x r + f-r T rw b ). 


(B.31) 


(B.32) 
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Appendix C 


Denavit-Hartenberg 
Parameters and Their 
Relationship to the Product 
of Exponentials 

C.l Denavit-Hartenberg Representation 

The basic idea underlying the Denavit-Hartenberg approach to forward kine¬ 
matics is to attach reference frames to each link of the open chain, and to 
derive the forward kinematics based on knowledge of the relative displacements 
between adjacent link frames. Assume that a fixed reference frame has been es¬ 
tablished, and that a reference frame (the end-effector frame) has been attached 
to some point on the last link of the open chain. For a chain consisting of n 
one degree of freedom joints, the links are numbered sequentially from 0 to n, 
in which the ground link is labeled 0, and the end-effector frame is attached to 
link n. Reference frames attached to the links are also correspondingly labeled 
from {0} (the fixed frame) to {n} (the end-effector frame). The joint variable 
corresponding to the i-th joint is denoted 9i. The forward kinematics of the 
n-link open chain can then be expressed as 

T 0n (6i, ...,e n ) = T 01 (e 1 )T 12 (e 2 ) ■ ■ ■ T n _ hn (e n ), (c.i) 

where i £ SE( 3) denotes the relative displacement between link frames 
{* — 1} and {i}. Depending on how the link reference frames have been chosen, 
each Ti-i^ can be obtained in a straightforward fashion. 
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Figure C.l: Illustration of Denavit-Hartenberg parameters. 


C.1.1 Assigning Link Frames 

Rather than attaching reference frames to each link in arbitrary fashion, in the 
Denavit-Hartenberg convention a set of rules for assigning link frames is ob¬ 
served. Figure C.l illustrates the frame assignment convention for two adjacent 
revolute joints i — 1 and i that are connected by link i — 1. 

The first rule is that the Zj-axis coincides with joint axis i , and z^_i coincides 
with joint axis i — 1. The direction of each link frame’s z-axis is determined via 
the right-hand rule, i.e., such that positive rotations are counterclockwise about 
the z-axis. 

Once the z-axis direction has been assigned, the next rule determines the 
origin of the link reference frame. First, find the line segment that orthogonally 
intersects both joint axes z^_i and zFor now let us assume that this line 
segment is unique; the case where it is not unique (i.e., when the two joint 
axes are parallel), or fails to exist (i.e., when the two joint axes intersect), is 
addressed later. Connecting joint axes i — 1 and i by a mutually perpendicular 
line, the origin of frame {i — 1} is then located at the point where this line 
intersects joint axis i — 1. 

Determining the remaining x- and y-axes of each link reference frame is now 
straightforward: the x axis is chosen to be in the direction of the mutually 
perpendicular line pointing from the i — 1 axis to the i axis. The y-axis is then 
uniquely determined from the cross-product x x y = z. Figure C.l depicts the 
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link frames {*} and {* — 1} chosen according to this convention. 

Having assigned reference frames in this fashion for links i and i— 1, we now 
define four parameters that exactly specify Tj_ 

• The length of the mutually perpendicular line, denoted by the scalar a,_i, 
is called the link length of link i — 1. Despite its name, this link length 
does not necessarily correspond to the actual length of the physical link. 

• The link twist ctj_i is the angle from %i-\ to z i5 measured about Xj_i- 

• The link offset di is the distance from the intersection of Xj_i and z i to 
the link i frame origin (the positive direction is defined to be along the Zj 
axis). 

• The joint angle (pi is the angle from x^_i to x,, measured about the 
z^-axis. 

These parameters constitute the Denavit-Hartenberg parameters. For an 
open chain with n one-degree-of-freedom joints, the 4n Denavit-Hartenberg pa¬ 
rameters are sufficient to completely describe the forward kinematics. In the 
case of an open chain with all joints revolute, the link lengths aj_i, twists ofj_i, 
and offset parameters d,; are all constant, while the joint angle parameters </>, 
act as the joint variables. 

We now consider the case where the mutually perpendicular line is undefined 
or fails to be unique, as well as when some of the joints are prismatic, and finally, 
how to choose the ground and end-effector frames. 

When Adjacent Revolute Joint Axes Intersect 

If two adjacent revolute joint axes intersect each other, then the mutually per¬ 
pendicular line between the joint axes fails to exist. In this case the link length 
is set to zero, and we choose x,_ i to be perpendicular to the plane spanned 
by z i_i and z i. There are two possibilities here, both of which are acceptable: 
one leads to a positive value of the twist angle at- 1 , while the other leads to a 
negative value. 

When Adjacent Revolute Joint Axes are Parallel 

The second special case occurs when two adjacent revolute joint axes are paral¬ 
lel. In this case there exist many possibilities for a mutually perpendicular line, 
all of which are valid (more precisely, a one-dimensional family of mutual per¬ 
pendicular lines is said to exist). A useful guide is to try to choose the mutually 
perpendicular line that is the most physically intuitive, and simplifies as many 
Denavit-Hartenberg parameters as possible (e.g., such that their values become 
zero). 
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%' 

/ 


Figure C.2: Link frame assignment convention for prismatic joints. Joint i — 1 
is a revolute joint, while joint * is a prismatic joint. 

Prismatic Joints 

For prismatic joints, the z-direction of the link reference frame is chosen to be 
along the positive direction of translation. This convention is consistent with 
that for revolute joints, in which the z-axis indicates the positive axis of rotation. 
With this choice the link offset di is the joint variable and the joint angle </>, is 
constant (see Figure C.2). The procedure for choosing the link frame origin, as 
well as the remaining x and y-axes, remains the same as for revolute joints. 

Assigning the Ground and End-Effector Frames 

Our frame assignment procedure described thus far does not specify how to 
choose the ground and final link frames. Here as before, a useful guideline is to 
choose initial and final frames that are the most physically intuitive, and simplify 
as many Denavit-Hartenberg parameters as possible. This usually implies that 
the ground frame is chosen to coincide with the link 1 frame in its zero (rest) 
position; in the event that the joint is revolute, this choice forces ao = ao = <^i = 
0, while for a prismatic joint we have do = cuo = 4>i = 0. The end-effector frame 
is attached to some reference point on the end-effector, usually at a location 
that makes the description of the task intuitive and natural, and also simplifies 
as many of the Denavit-Hartenberg parameters as possible (e.g., their values 
become zero). 

It is important to realize that arbitrary choices of the ground and end- 
effector frames may not always be possible, since there may not exist a valid set 
of Denavit-Hartenberg parameters to describe the relative transformation. We 
elaborate on this point below. 
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A 




A 



Zb 


A 

Y b 


{a} 


{b} 


Figure C.3: An example of three frames {a}, {b}, and {c}, in which the trans¬ 
formations T a b and T ac cannot be described by any set of Denavit-Hartenberg 
parameters. 

C.l.2 Why Four Parameters are Sufficient 

In our earlier study of spatial displacements, we argued that a minimum of 
six independent parameters were required to describe the relative displacement 
between two frames in space: three for the orientation, and three for the po¬ 
sition. Based on this result, it would seem that for an n-link arm, a total of 
6n parameters would be required to completely describe the forward kinematics 
(each in the above equation would require six parameters). Surprisingly, 

in the Denavit-Hartenberg parameter representation only four parameters are 
required for each transformation Tj_i j. Although this result may at first appear 
to contradict our earlier results, this reduction in the number of parameters is 
accomplished by the carefully stipulated rules for assigning link reference frames. 
If the link reference frames had been assigned in arbitrary fashion, then more 
parameters would have been required. 

Consider, for example, the link frames shown in Figure C.3. The trans¬ 
formation from frame {a} to frame {b} is a pure translation along the y-axis 
of frame {a}. If one were to try to express the transformation T a & in terms 
of the Denavit-Hartenberg parameters (a, a, d, 0) as prescribed above, it should 
become apparent that no such set of parameter values exist. Similarly, the trans¬ 
formation T ac also does not admit a description in terms of Denavit-Hartenberg 
parameters, as only rotations about the x- and z- axes are permissible. Under 
our Denavit-Hartenberg convention, only rotations and translations along the 
x and z axes are allowed, and no combination of such motions can achieve the 
transformation shown in Figure C.3. 

Given that the Denavit-Hartenberg convention uses exactly four parameters 
to describe the transformation between link frames, one might naturally wonder 
if the number of parameters can be reduced even further, by an even more clever 
set of link frame assignment rules. Denavit and Hartenberg show that this is 
not possible, and that four is the minimum number of parameters [24]. 

We end this section with a reminder that there are alternative conventions 
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for assigning link frames. Whereas we chose the z-axis to coincide with the joint 
axis, some authors choose the x-axis, and reserve the z-axis to be the direction 
of the mutually perpendicular line. To avoid ambiguities in the interpretation 
of the Denavit-Hartenberg parameters, it is essential to include a concise de¬ 
scription of the link frames together with the parameter values. 


C.1.3 Manipulator Forward Kinematics 

Once all the transformations between adjacent link frames are known 

in terms of their Denavit-Hartenberg parameters, the forward kinematics is 
obtained by sequentially multiplying these link transformations. Each link frame 
transformation is of the form 


= Rot(x, ai_i)Trans(x, aj_i)Trans(z, di)Rot(z, <^j) 


where 


cos (pi 

sin (pi cos on -1 
sint/jj sin cti_i 
0 


Rot(x, ai-i) = 


Trans (x, eq_i) = 


Trans(z, di) = 


Rot(z, (p{) = 


— sin (pi 

COS (pi COS OLi — i 

cos (pi sin ccj-i 

0 


0 

— sin ai -1 

COS ttj-i 
0 


di-1 

—di sin (Xi-\ 
di cos Q!j_i 

1 


0 

COS Oii -1 

— sin ai -1 

0 


0 

— sin ai -1 
cos i 
0 


0 0 
1 0 
0 1 
0 0 

0 0 
1 0 
0 1 
0 0 


COS (pi -1 

- sin <pi-i 
0 
0 


ai-1 
0 
0 
1 

0 
0 
di 
1 

— sin (pi -1 

COS (pi- 1 

0 
0 


(C.2) 


(C.3) 


(C.4) 


(C.5) 


A useful way to visualize Tj^-i is to transport frame {i — 1} to frame {i} via 
the following sequence of four transformations: 

(i) Rotate frame {* — 1} about its x axis by an angle aij_i. 

(ii) Translate this new frame along its x axis by a distance a^_i. 


(iii) Translate this new frame along its z axis by a distance di. 

(iv) Rotate this new frame about its z axis by an angle (pi. 
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Note that switching the order of the first and second steps will not change the 
final form of Ti_ Similarly, the order of the third and fourth steps can also 
be switched without affecting Tj_I,*. 

C.l.4 Examples 

We now derive the Denavit-Hartenberg parameters for some common spatial 
open chain structures. 

Example: A 3R Spatial Open Chain 

Consider the 3R spatial open chain of Figure 4.3, shown in its zero position 
(i.e., with all its joint variables set to zero). The assigned link reference frames 
are shown in the figure, and the corresponding Denavit-Hartenberg parameters 
listed in the following table: 


i 

Oii-l 

Oi-l 

di 

0i 

1 

0 

0 

0 

0i 

to 

O 

O 

Oi 

Li 

0 

e 2 - 90° 

3 

-90° 

L2 

0 

o 3 


Note that frames {1} and {2} are uniquely specified from our frame assign¬ 
ment convention, but that we have some latitude in choosing frames { 0 } and 
{3}. Here we choose the ground frame {0} to coincide with frame {1} (resulting 
in ao = ao = d\ = 0), and frame {3} such that X 3 = X 2 (resulting in no offset 
to the joint angle 63 ). 

Example: A Spatial RRRP Open Chain 

The next example we consider is the four degree-of-freedom RRRP spatial open 
chain of Figure C.4, here shown in its zero position. The link frame assignments 
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are as shown, and the corresponding Denavit-Hartenberg parameters are listed 
in the following table: 


i 

G^l 

o»-1 

di 

4 *i 

1 

0 

0 

0 

9 i 

2 

QD 

O 

0 

0 

0 

92 

3 

0 

L2 

0 

9 3 + 90° 

4 

90° 

0 

9 a 

0 


The four joint variables are (0 l7 0 2 , $ 3 , $ 4 ), where 64 is the displacement of 
the prismatic joint. As in the previous example, the ground frame {0} and final 
link frame {4} have been chosen to make as many of the Denavit-Hartenberg 
parameters zero. 

Example: A Spatial 6R Open Chain 

The final example we consider is a widely used six 6 R robot arm (Figure C.5). 
This open chain has six rotational joints: the first three joints function as a 
Cartesian positioning device, while the last three joints act as a ZYZ Euler 
angle-type wrist. The link frames are shown in the figure, and the corresponding 
Denavit-Hartenberg parameters are listed in the following table: 


i 

OLi-l 

O-i-l 

di 

02 

1 

0 

0 

0 

9 i 

2 

O 

O 

05 

0 

0 

92 

3 

0 

Li 

0 

0 3 + 90° 

4 

O 

O 

O 

0 

L2 

04 + 180° 

5 

O 

O 

05 

0 

0 

0 5 + 180° 

6 

O 

O 

05 

0 

0 

9 e 
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C.l.5 Relation between the Product of Exponential and the 
Denavit-Hartenberg Representations 

The product of exponentials formula can be derived directly from the Denavit- 
Hartenberg parameter-based representation of the forward kinematics. As be¬ 
fore, denote the relative displacement between adjacent link frames by 

Ti-i t i = Rot(x, «i_i)Trans(x, Oi_i)Trans(z, di)Rot(z, ^>j). 


If joint i is revolute, the first three matrices can be regarded as constant, and 
c f>i becomes the revolute joint variable. Define 9i = fa, and 

Mi = Rot(x, aij_i)Trans(x, ai_i)Trans(z, fa), (C.6) 


and write Rot(z,0i) as the following matrix exponential: 


Rot(z, 9i) = e^ Ai ^ 6i , [Ai} 


0-100 

1 0 0 0 

0 0 0 0 

0 0 0 0 


(C.7) 


With the above definitions we can write T)_ 

If joint i is prismatic, then fa becomes the joint variable, fa is a constant 
parameter, and the order of Trans(z, fa) and Rot(z, fa) in can be reversed 

(recall that reversing translations and rotations taken along the same axis still 
results in the same motion). In this case we can still write = Mie 

where 9i = fa and 


Mi 


[■Ai\ 


Rot(x, aj_i)Trans(x, ai_i)Rot(z, <pi) 

' 0 0 0 0 ' 

0 0 0 0 

0 0 0 1 ' 

0 0 0 0 


(C.8) 

(C.9) 


Based on the above, for an n-link open chain containing both revolute and 
prismatic joints, the forward kinematics can be written 

T 0i „ = M ^ 1 ^ 1 M 2 e [A2]f>2 ■ ■ ■ M n e [An]e " (C.10) 


where 9i denotes joint variable i, and [Ai] is either of the form (C.7) or (C.9) 
depending on whether joint i is revolute or prismatic. 

We now make use of the matrix identity Me p M~ 1 = e MPM , which holds 
for any nonsingular M £ K” XTl and arbitrary P £ R” xn . The above can also be 
rearranged as Me p = e MPM M. Beginning from the left of Equation (C.10), 
if we repeatedly apply this identity, after n iterations we obtain the product of 
exponentials formula as originally derived: 

T 0n = e Ml[A ^ M i l 0 faM 1 M 2 )e [A2]e2 ■■■e [A A e ^ 

— e M 1 [A 1 ]Mp 1 9 1 e (M 1 M 2 )[A2](M 1 M 2 )~ 1 S 2 (MiM 2 M 3 ) e^ 3 !® 3 • • • 

= e [Sl]9l ---e (C.ll) 
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where 


[5,] = i = l,...,n (C.12) 

M = (C.13) 

We now re-examine the physical meaning of the St by recalling how a screw 
twist transforms under a change of reference frames. If S a represents the screw 
twist for a given screw motion with respect to frame {a}, and S b represents the 
screw twist for the same physical screw motion but this time with respect to 
frame {&}, then recall that S a and Sb are related by 

[S b ]=T ba [S a ]T b -\ (C.14) 

or using the adjoint notation AdT ta , 

S b = Ad Tba (S a ). (C.15) 

Seen from the perspective of this transformation rule, Equation (C.13) suggests 
that Ai is the screw twist for joint axis i as seen from link frame {*}, while Si 
is the screw twist for joint axis i as seen from the fixed frame {0}. 
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